Pathfinding working.

Speeds are inconsistent and on occasion the agent moves outwith the
bounds of the mesh.