Added a second test nav graph
Fixed bugs in path search