// Source: https://en.wikipedia.org/wiki/A*_search_algorithm #include "astar.h" #ifndef NULL #define NULL ((void *) 0) #endif enum SearchSet { ASTAR_SET_NONE = 0, ASTAR_SET_OPEN, ASTAR_SET_CLOSED }; enum { ASTAR_INFINITY = 0xFFFFu }; void astar_initNode(AStarNode *node, void *userData, byte canGoEast, byte canGoSouth) { node->userData = userData; node->canGoEast = canGoEast; node->canGoSouth = canGoSouth; } static size_t astar_distBetween(size_t x0, size_t y0, size_t x1, size_t y1) { size_t dx = x1 - x0, dy = y1 - y0; return sqrt16(dx * dx + dy * dy); } static size_t astar_estimateHeuristicCost(AStarGraph *graph, size_t x0, size_t y0, size_t x1, size_t y1) { return astar_distBetween(x0, y0, x1, y1); } enum AStarDirection { NORTH, EAST, SOUTH, WEST }; typedef struct AStarCouple { char dx, dy; } AStarCouple; static AStarCouple fourDirections[] = { { 0, -1 }, // north { +1, 0 }, // east { 0, +1 }, // south { -1, 0 }, // west }; static BOOL astar_getLowestFScoreInOpenSet(AStarGraph *graph, size_t *indexOfLowest, size_t *lowestX, size_t *lowestY) { size_t w = graph->width; size_t numNodes = w * graph->height; word min_fScore = ASTAR_INFINITY; *indexOfLowest = numNodes; AStarNode *begin = graph->nodes; AStarNode *end = &graph->nodes[numNodes]; for (AStarNode *node = begin; node != end; ++node) { if (node->searchSet != ASTAR_SET_OPEN) continue; word f = node->fScore; if (f < min_fScore) { min_fScore = f; *indexOfLowest = node - begin; } } if (*indexOfLowest == numNodes) return FALSE; // none found divmod16(*indexOfLowest, w, lowestY, lowestX); return TRUE; // found } static BOOL astar_areNeighborsReachable(AStarNode *start, AStarNode *end, byte dir) { switch (dir) { case EAST: return start->canGoEast; case SOUTH: return start->canGoSouth; case NORTH: return end->canGoSouth; case WEST: return end->canGoEast; } return FALSE; // should not get here } // graph: Array of nodes in row-major order. // // Each node must have been initialized by astar_initNode() before // calling this function. // BOOL astar_searchGrid(AStarGraph *graph, size_t startX, size_t startY, size_t goalX, size_t goalY) { size_t numNodes = graph->width * graph->height; AStarNode *begin = graph->nodes; AStarNode *end = &graph->nodes[numNodes]; for (AStarNode *node = begin; node != end; ++node) { node->searchSet = ASTAR_SET_NONE; node->gScore = ASTAR_INFINITY; node->fScore = ASTAR_INFINITY; node->cameFrom = NULL; } AStarNode *startNode = &graph->nodes[graph->width * startY + startX]; startNode->searchSet = ASTAR_SET_OPEN; startNode->gScore = 0; startNode->fScore = astar_estimateHeuristicCost(graph, startX, startY, goalX, goalY); size_t currentIndex, currentX, currentY; while (astar_getLowestFScoreInOpenSet(graph, ¤tIndex, ¤tX, ¤tY)) { AStarNode *currentNode = &graph->nodes[currentIndex]; if (currentX == goalX && currentY == goalY) { return TRUE; // success } currentNode->searchSet = ASTAR_SET_CLOSED; // also removes node from open set AStarCouple *c = fourDirections; for (byte dir = 0; dir < 4; ++dir, ++c) // for each neighbor of 'currentNode' { size_t neighborX = currentX + c->dx; if (neighborX >= graph->width) continue; // out of grid size_t neighborY = currentY + c->dy; if (neighborY >= graph->height) continue; // out of grid AStarNode *neighbor = &graph->nodes[graph->width * neighborY + neighborX]; if (neighbor->searchSet == ASTAR_SET_CLOSED) continue; // already visited // Can we go from 'currentNode' to 'neighbor'? if (!astar_areNeighborsReachable(currentNode, neighbor, dir)) continue; // no, not an actual neighbor neighbor->searchSet = ASTAR_SET_OPEN; size_t tentative_gScore = astar_distBetween(currentX, currentY, neighborX, neighborY); word current_gScore = currentNode->gScore; if (tentative_gScore > ASTAR_INFINITY - current_gScore) // if overflow tentative_gScore = ASTAR_INFINITY; else tentative_gScore += current_gScore; if (tentative_gScore >= neighbor->gScore) continue; // not a better path neighbor->cameFrom = currentNode; neighbor->gScore = tentative_gScore; neighbor->fScore = tentative_gScore + astar_estimateHeuristicCost(graph, neighborX, neighborY, goalX, goalY); } // for } // while return FALSE; // failure } void astar_startPathIteration(AStarPathIterator *iter, AStarGraph *graph, size_t goalX, size_t goalY) { iter->graph = graph; iter->node = &graph->nodes[goalY * graph->width + goalX]; } AStarNode * astar_advancePathIteration(AStarPathIterator *iter) { AStarNode *current = iter->node; if (!current) // if already at end return 0; iter->node = current->cameFrom; // for next call to this function return current; }