Bonus: Build once, query many

RRT forgets everything

Each RRT grows a tree for one query and throws it away. Clearing four balls takes eight transits, and most of them cross the same stretch of space over the wall. A probabilistic roadmap (PRM) does that exploration once and reuses it:

1. Build (once): sample $N$ free configurations, and join each one to its $k$ nearest neighbours wherever the straight edge is free. The result is a graph of safe motions through C-space.
2. Query (every transit): join the start and the goal to their nearest roadmap nodes the same way, then search the graph for the shortest route.

Dijkstra's algorithm

Keep a priority queue of (distance so far, node), starting with (0, a). Repeatedly pop the closest node $i$ and relax each of its edges:

Whenever $d(j)$ improves, record $i$ as $j$'s predecessor and push $j$. When the goal comes off the queue, walk the predecessors back. Python's heapq is the priority queue.

When the graph falls apart

If the start and goal land in separate pieces of the graph, the roadmap is too sparse there. Sample some more nodes and search again. PRM is probabilistically complete too.

Your task

Clear all four balls using one roadmap. add_node(q) is written for you. Implement build_roadmap(n), shortest_path(a, b) and plan(q_start, q_goal). Right now plan returns the straight line.

Bonus: lazy edges. Checking every edge up front costs as much as four RRT searches (about 45,000 collision checks), yet most edges are never used. Build with add_node(q, check=False), and in plan check only the edges on the route Dijkstra returns. Delete a blocked edge and search again. (The grey roadmap is then optimistic: some of its edges cross the wall until a route tries them.) Finish under 2,000 checks. Every in_collision call counts, and so does every configuration segment_free tests.