One Branch Toward the Sample

A planetary rover plans its drive with a rapidly-exploring random tree (RRT). The tree is a list of points nodes, positions (x, y) in metres that the rover knows it can reach. Each round the planner draws a random sample point and grows the tree a little toward it:

1. Find the nearest node to the sample (straight-line distance). If several nodes are equally near, take the one with the lowest index.
2. From that node, move toward the sample by at most step metres: if the sample is no farther than step from the node, the new point is the sample itself; otherwise it is the point step metres from the node along the straight line to the sample.

Write rrt_extend(nodes, sample, step) that returns the tuple (index, new_point): the index of the nearest node and the new point as a tuple (x, y).

Examples

Input:  nodes = [(0, 0), (2, 0), (2, 2)], sample = (5, 1), step = 1
Output: (1, (2.948683298050514, 0.31622776601683794))
Explanation: nodes 1 and 2 are both sqrt(10) = 3.1623 from the sample; node 1 has the
lower index. One metre from (2, 0) toward (5, 1) is (2, 0) + (3, 1) / sqrt(10).

Input:  nodes = [(1, 1)], sample = (1.5, 1.2), step = 1
Output: (0, (1.5, 1.2))
Explanation: the sample is only 0.5385 m away, closer than one step: take it as it is.

Input:  nodes = [(0, 0), (4, 0)], sample = (2, 0), step = 0.5
Output: (0, (0.5, 0.0))

Constraints

• answers are compared with a tolerance of 1e-6

Goals

• Find the tree node nearest to a sample, breaking ties by the lower index
• Grow at most one step of fixed length from that node toward the sample