Pathfinding

This is a dijkstra based 3d pathfinding algorithm.