Pathfinding made 3D compliant (Straight line, A*)