CSpace-Aware Graph

[elvout]

Graph search tends to find paths that are infeasible due to the way MapGraph is constructed.

In particular, these paths:

• violate kinematic constraints of the car (e.g. by moving sideways)
• are too close to map obstacles, often "hugging" walls

Both of these issues can be mitigated by constructing the graph as CSpace-aware.

Issue 1

For simplicity, we can keep the graph 8-connected. Each vertex would contain some angular information (can be implemented as enum) and edges between vertices will have angular displacement constraints to more closely resemble the turning constraints of the car.

Problems

• This approach grows the size of our graph by 8 times.
• Does the octile distance heuristic still make sense?

Issue 2

A proper CSpace-aware graph would take into account the entire CSpace domain, but I think a simple model would work for our case. Since the car is moving along the x-axis most of the time, we can probably just use half the width of the car (maybe + some additional padding) to inflate each obstacle in the map. The local navigation system has collision avoidance as well.

For the simplified approach, we don't necessarily need to store all 8 angle possibilities for each obstacle since the position of the obstacle is most significant. However, there may be a runtime penalty converting Vector3i to Vector2i to check for set membership.

Issue 3

Identified in #69.

Wall representation in the MapGraph obstacle set leaves holes that search can expand through. Some walls also just aren't represented as bounded regions in the vector map.

[elvout]

Oh actually because of the octile heuristic, graph search generally prefers moving diagonally rather than sideways. The solution proposed for issue 1 may not be necessary, although the plan sometimes creates very sharp turns.

[elvout]

The resolution of the graph presents a problem.

As an example, for a coarse enough graph, it's possible for a doorway to be entirely within adjacent discretized vertices:

WWWWWWWWWWWWWWWWW
W            +--W--+
W            |  W  |
W            |  W  |
W            |     |
W    CAR->   +-----+    GOAL
W            |     |
W            |  W  |
W            |  W  |
W            +--W--+
WWWWWWWWWWWWWWWWW

For a path to exist from the car to the goal, there must be an empty bin in the frame of the door.

Intuitively, we should just increase the resolution of the graph, but this will increase both time and space complexity. We'll probably need to implement some optimization like JPS.