Get some feedback on polypath

Save g_val and h_val in navmesh and display in navmesh_test