Added NavMesh2D triangle visualization and fixed some nav mesh bugs