int *astar_compute (const char *grid,
int *solLength,
int boundX,
int boundY,
int start,
int end)
{
*solLength = -1;
astar_t astar;
coord_t bounds = {boundX, boundY};
int size = bounds.x * bounds.y;
if (start >= size || start < 0 || end >= size || end < 0)
return NULL;
coord_t startCoord = getCoord (bounds, start);
coord_t endCoord = getCoord (bounds, end);
if (!contained (bounds, startCoord) || !contained (bounds, endCoord))
return NULL;
queue *open = createQueue();
char closed [size];
double gScores [size];
int cameFrom [size];
astar.solutionLength = solLength;
astar.bounds = bounds;
astar.start = start;
astar.goal = end;
astar.grid = grid;
astar.open = open;
astar.closed = closed;
astar.gScores = gScores;
astar.cameFrom = cameFrom;
memset (closed, 0, sizeof(closed));
gScores[start] = 0;
cameFrom[start] = -1;
insert (open, start, estimateDistance (startCoord, endCoord));
while (open->size) {
int node = findMin (open)->value;
coord_t nodeCoord = getCoord (bounds, node);
if (nodeCoord.x == endCoord.x && nodeCoord.y == endCoord.y) {
freeQueue (open);
return recordSolution (&astar);
}
deleteMin (open);
closed[node] = 1;
direction from = directionWeCameFrom (&astar,
node,
cameFrom[node]);
directionset dirs =
forcedNeighbours (&astar, nodeCoord, from)
| naturalNeighbours (from);
for (int dir = nextDirectionInSet (&dirs); dir != NO_DIRECTION; dir = nextDirectionInSet (&dirs))
{
int newNode = jump (&astar, dir, node);
coord_t newCoord = getCoord (bounds, newNode);
// this'll also bail out if jump() returned -1
if (!contained (bounds, newCoord))
continue;
if (closed[newNode])
continue;
addToOpenSet (&astar, newNode, node);
}
}
freeQueue (open);
return NULL;
}
int *astar_compute (const char *grid,
int *solLength,
int boundX,
int boundY,
int start,
int end)
{
astar_t astar;
if (!init_astar_object (&astar, grid, solLength, boundX, boundY, start, end))
return NULL;
struct coord_t bounds = {boundX, boundY};
struct coord_t endCoord = getCoord (bounds, end);
while (astar.open->size) {
int dir;
int node = findMin (astar.open)->value;
struct coord_t nodeCoord = getCoord (bounds, node);
if (nodeCoord.x == endCoord.x && nodeCoord.y == endCoord.y) {
freeQueue (astar.open);
free (astar.closed);
free (astar.gScores);
int *rv = recordSolution (&astar);
free (astar.cameFrom);
return rv;
}
deleteMin (astar.open);
astar.closed[node] = 1;
direction from = directionWeCameFrom (&astar,
node,
astar.cameFrom[node]);
directionset dirs =
forcedNeighbours (&astar, nodeCoord, from)
| naturalNeighbours (from);
for (dir = nextDirectionInSet (&dirs); dir != NO_DIRECTION; dir = nextDirectionInSet (&dirs))
{
int newNode = jump (&astar, dir, node);
struct coord_t newCoord = getCoord (bounds, newNode);
// this'll also bail out if jump() returned -1
if (!contained (bounds, newCoord))
continue;
if (astar.closed[newNode])
continue;
addToOpenSet (&astar, newNode, node);
}
}
freeQueue (astar.open);
free (astar.closed);
free (astar.gScores);
free (astar.cameFrom);
return NULL;
}
