Further improvements on the navmesh (diagonals)