
The exact same algorithm works beautifully for planning the shortest path

to a single mission goal from any possible start location,

and the only difference here is in this graph over here of an actual road graph

we are also incorporating the heading direction as measure of distance.

Green corresponds to nearby to large values, red to far away.

The reason why the area below the mission goal is green is because we expect

the car to point up, to point north, at the time it reached the mission.

So if it came from the north, it would point in the wrong direction.

The state space is augmented correspondingly.

Where if it comes from over here, it points in the correct direction.

If you look at the circle over here, it's interesting.

If we came from the left side over here, it could do a right turn,

but over here it's forced on this oneway circle to do the entire loop to go around,

and that increases the value as it comes over here.

This is value iteration applied to the road graph where we keep track of heading

and where the circle over here is a oneway circle.