Graphs & paths: A*, getting out of a maze.

Now define the heuristic, the euclidean distance (the distance formula). This calculation guides the algorithm from the current point to the goal.

float euclideanDistance(point a, point b) {
return (pow( pow( a.x - b.x, 2.0) + pow( a.y - b.y, 2.0), 0.5));
}

Now thatpoint and euclideanDistance are defined, let’s generate a random maze.

void randomMaze(int maze[HEIGHT][WIDTH], point p) {
point rn[4] = {
point(p.x-2, p.y, direction::L),
point(p.x+2, p.y, direction::R),
point(p.x, p.y+2, direction::U),
point(p.x, p.y-2, direction::D)
};
std::random_shuffle(&rn[0], &rn[4]);
for(point cn : rn) {
if(cn.inBounds() && !maze[cn.y][cn.x]) {
if(cn.d == direction::L)
maze[cn.y][cn.x+1] = 1;
else if(cn.d == direction::R)
maze[cn.y][cn.x-1] = 1;
else if(cn.d == direction::U)
maze[cn.y-1][cn.x] = 1;
else if(cn.d == direction::D)
maze[cn.y+1][cn.x] = 1;
maze[cn.y][cn.x] = 1;
randomMaze(maze, cn);
}
}
}

This code is interesting, because this code generates a random maze by using the recursive call stack.

The code starts by creating an array of second degree neighbor points. Next the code shuffles those points in a random ordering. Then the code iterates on each second degree neighbor. If the second degree neighbor is inBounds and the point is a wall; convert the point to a path, convert the first degree neighbor to a path, and make a recursive call on the second degree neighbor.

*Note, walls are the value 0 and paths are the value 1 .

Now point, euclideanDistance, and randomMaze are defined. Let’s code A*.

std::vector<points> astar(int maze[HEIGHT][WIDTH],point s,point g) {
//initialize sets//
std::vector<point> paths[HEIGHT][WIDTH];
float dist[HEIGHT][WIDTH] = { 0 };
bool visited[HEIGHT][WIDTH] = { 0 };
for(int i=0; i<HEIGHT; i++)
for(int j=0; j<WIDTH; j++)
dist[i][j] = INT_MAX;

 //initialize starting point//
point cur = s;
dist[cur.y][cur.x] = euclideanDistance(s,g);

 //best-fit search algorithm//
while( !(cur == g) ) {
//update current point to being visited//
visited[cur.y][cur.x] = 1;

 //neighbors of the current point//
point nb[4] = {
point(cur.x-1,cur.y,direction::L),
point(cur.x+1,cur.y,direction::R),
point(cur.x,cur.y-1,direction::U),
point(cur.x,cur.y+1,direction::D)
};

 //calculate distances//
for(point cn : nb )
if( cn.inBounds() && maze[cn.y][cn.x] &&
(euclideanDistance(cn,g) + dist[cur.y][cur.x] + maze[cn.y][cn.x] < dist[cn.y][cn.x]) ) {
dist[cn.y][cn.x] = euclideanDistance(cn,g) + dist[cur.y][cur.x] + maze[cn.y][cn.x];
paths[cn.y][cn.x] = paths[cur.y][cur.x], paths[cn.y][cn.x].push_back(cur);
} 

 //select point of next iteration//
cur = point(-1,-1);
float md = INT_MAX;
for(int i=0; i<HEIGHT; i++)
for(int j=0; j<WIDTH; j++)
if(!visited[i][j] && dist[i][j]!=INT_MAX && dist[i][j] < md) { cur = point(j,i), md = dist[i][j]; }
}

 //return path from start to goal//
paths[g.y][g.x].push_back(g);
return paths[g.y][g.x];

}

A* starts by initializing sets for distance, visited, and paths. Initially all distances are INT_MAX (unreachable) and all visited points are 0 (unvisited). Set the cur point to the starting point, s, and set the distance at cur to the euclideanDistance. Now walk the maze.

For each iteration set visited at the cur point to 1 and then calculate the distances to the neighboring points.

To calculate the distances to the neighbor points; take the distance at the cur point add the distance of the neighbor point and add the euclideanDistance (from the neighbor point to the goal).

If this calculated distance is less than the currently assigned distance, update the distance.

The last step is selecting the next move. Select the smallest distance value that is reachable and unvisited. When the current point, cur, is at the goal point, g, the algorithm is complete.

Note, A* is Dijkstra’s algorithm + heuristic. For more explicit steps implementing Dijkstra’s algorithm read this now.