Pathfinding Update

Variable height pathifinding