Basic algorithms for height map based 3D path planning: BFS, Dijkstra, A*, Theta* - View it on GitHub