Pathfinding

This is a dijkstra based 3d pathfinding algorithm as well as two variants of A* algorithm. While A* not always results in shortest path it'll be almost as good for most cases and way faster.

See this for a performance comparison between different variants:
http://ompldr.org/vaHNkcw/performance_comparison_pathfinding.pdf