Quick implementation of NavMesh and pathfinding