Some work on navmesh testing, note that it isnt working yet