Navigation/A* testing in progress, need to deal with origin problem.