implement cut corners algorithm for navmesh path
better comments and formatting