The following change makes the code much faster as it only searches where it needs to. You might also add a tie-breaker.
inline int computeH(MapNode *node1, MapNode *node2){
if (ALLOW_VERTEX_PASSTHROUGH) {
return diagonal_distance(node1, node2)*G_SKEW;
} else {
return manhattan_distance(node1, node2)*G_DIRECT;
}
}
/** The standard heuristic is the Manhattan distance.
* Look at your cost function and see what the least cost is
* for moving from one space to another.
* The heuristic should be cost times manhattan distance: */
inline int manhattan_distance(MapNode* node1, MapNode* node2){
return abs(node2->x - node1->x) + abs(node2->y - node1->y);
}
/** If on your map you allow diagonal movement, then you need a different heuristic.
* The Manhattan distance for (4 east, 4 north) will be 8.
* However, you could simply move (4 northeast) instead, so the heuristic should be 4.
* This function handles diagonals: */
inline int diagonal_distance(MapNode* node1, MapNode* node2){
return max(abs(node2->x - node1->x),abs(node2->y - node1->y));
}