Problem 170379 · medium · Robotics · Level 01 Poses and Frames

The Sampling Rover Picks the Nearest Rock

pose · greedy · nearest neighbour · tie-breaking · atan2

A planetary rover must drill a sample from each of several rocks. Its controller uses a simple rule to choose the order: from where it stands, it drives to the nearest rock it has not yet sampled, samples it, and repeats until every rock is done.

The rover starts at the pose start = (x, y, theta): a position on a metre grid (x to the right, y up; whole numbers) and a heading in radians, counter-clockwise from the +x axis. It turns on the spot to face the next rock and drives straight to it, so after each leg its heading is the direction of that leg (a leg of length 0 leaves the heading unchanged). The rocks are a list of whole-number positions rocks[i] = (rx, ry); two rocks may share a position.

Choosing the next rock:

  1. the smallest straight-line distance from the rover's current position;
  2. among equally near rocks, the one needing the smaller turn: the size of the shorter turn from the rover's current heading to the direction of the rock, from 0 to \pi (a rock at the rover's own position needs no turn). Turns that differ by less than 1e-9 count as equal;
  3. then the smaller index.

Write sampling_order(start, rocks) that returns a tuple (order, distance): the list of rock indices in the order they are sampled, and the total distance driven in metres.

Examples

Input:  start = (0, 0, -0.3), rocks = [(5, 0), (0, 2), (0, -2), (3, 0)]
Output: ([2, 3, 0, 1], 12.990716082598492)
Explanation: rocks 1 and 2 are both 2 m away; facing -0.3, the rover turns 1.2708 to face
rock 2 but 1.8708 to face rock 1, so rock 2 is first. From (0, -2) rock 3 is nearest
(sqrt 13), then rock 0 (2 m), then rock 1 (sqrt 29): 2 + 3.6056 + 2 + 5.3852 m.

Input:  start = (0, 0, 0), rocks = [(0, -1), (0, 1), (1, 1)]
Output: ([0, 1, 2], 4.0)
Explanation: rocks 0 and 1 are equally near and need equal quarter turns, so the smaller index wins.

Input:  start = (2, 2, 1.0), rocks = []
Output: ([], 0.0)

The nearest-rock rule is quick but does not always give the shortest route. Press Run with plot(xs, ys) on the visited positions to see the route it makes.

Constraints

  • the distance is compared with a tolerance of 1e-6; do not round

Goals

  • Build a visiting order greedily: always the nearest unvisited waypoint next
  • Carry the robot's heading along the route, because it decides ties
  • Remove visited waypoints so each is visited exactly once
Starting Python…