A* Routing Algorithm

In computer science, A* is a computer algorithm that is widely used in pathfinding and graph traversal, the process of plotting an efficiently traversable path between points, called nodes. Noted for its performance and accuracy, it enjoys widespread use. However, in practical travelrouting systems, it is generally outperformed by algorithms which can preprocess the graph to attain better performance.
A* uses a bestfirst search and finds a leastcost path from a given initial node to one goal node (out of one or more possible goals). As A* traverses the graph, it follows a path of the lowest expected total cost or distance, keeping a sorted priority queue of alternate path segments along the way.
It uses a knowledgeplusheuristic cost function of node x (usually denoted f(x)) to determine the order in which the search visits nodes in the tree. The cost function is a sum of two functions:
1. the past pathcost function, which is the known distance from the starting node to the current node x (usually denoted g(x))
2. a future pathcost function, which is an admissible "heuristic estimate" of the distance from x to the goal (usually denoted h(x)).
In order to demonstrate, how A* works, I created hilly heightmesh using OpenGL. My application finds shortest path between to set points in the mountains. Just before we calculate path, we need to distinguish, which nodes are passable and which are not. So we create height mesh and then check, if angle between two neighbour nodes is higher than 70 degrees  node is unpassable. Only passable nodes will be included in our search. Later, we use heuristic in order to calculate possible route. I am using Euclidian function, but I also included Manhattan and Chebyshev functions in order to compare searching speed. Algorithm uses Best First Search (BFS).
Here is how the generated height mesh is looking:
In my program you are able to move coordinate axis with arrow keys, also to switch between wire and normal modes with key W.
You can download source, working with Windows and MacOS X prior to 10.9, here:
MAIN
HEIGHT MESH
HEIGHT MESH HEADER
HEIGHT MESH PRIVATE