A similar algorithm I had used when I drove with GPS 72 and paper roads map. I love GPS very much from this time.
P.S. That is interesting. When I was obliged to play to soldier we have a equipments what have a worth about a million for the solution of similar problems as in this thread!