Big update, got the navmesh working