Started NavMesh code; closest point on a triangle can now be found