Paths grown by a sampling planner wiggle. A rover tidies its path of waypoints path (a list of points (x, y) in metres, from start to goal) by trying shortcuts, among boulders obstacles (discs (cx, cy, r)).
A segment between two points is clear when no boulder comes within or onto it: for every boulder the shortest distance from its centre to the segment is more than r (exactly r counts as touching, which is not clear).
pairs is a list of index pairs (i, j) with i < j, indices into the original path. Try them in order. A shortcut (i, j) is taken when both waypoints i and j are still on the path and the segment from path[i] to path[j] is clear; taking it removes every waypoint with an index strictly between i and j (some may be gone already). Otherwise nothing changes.
The length of a path is the sum of the straight-line distances between consecutive waypoints (0.0 for a single waypoint).
Write shortcut_path(path, obstacles, pairs) that returns (kept, before, after): the indices of the waypoints still on the path in increasing order, the length of the original path, and the length of the shortened one.
Examples
Input: path = [(0, 0), (1, 1), (2, 0), (3, 1), (4, 0)], obstacles = [], pairs = [(0, 2), (2, 4)]
Output: ([0, 2, 4], 5.656854249492381, 4.0)
Explanation: four segments of sqrt(2) each become two of length 2.
Input: path = [(0, 0), (1, 1), (2, 0), (3, 1), (4, 0)], obstacles = [(1, 0.2, 0.5)],
pairs = [(0, 2), (1, 3), (0, 4)]
Output: ([0, 1, 3, 4], 5.656854249492381, 4.82842712474619)
Explanation: (0, 2) passes 0.2 m from the boulder's centre: not clear. (1, 3) stays
0.8 m away and drops waypoint 2. (0, 4) runs along the x axis through the boulder.
Input: path = [(0, 0), (1, 1), (2, 0), (3, 1), (4, 0)], obstacles = [], pairs = [(1, 3), (2, 4), (0, 3)]
Output: ([0, 3, 4], 5.656854249492381, 4.576491222541475)
Explanation: once (1, 3) has removed waypoint 2, the pair (2, 4) no longer applies.
Constraints
- the waypoints of
pathare all different; answers are compared with a tolerance of1e-6
Goals
- Measure a path of waypoints as the sum of its straight segments
- Shortcut a path: drop the waypoints between two that see each other
- Keep track of which waypoints are still on the path as shortcuts accumulate