Original author: Pierre Sarrazin. Public-domain package. Source: http://sarrazip.com/dev/astar-0.1.0.tar.gz Archive SHA-256: acd2d5e2ad709892cb7609f5e53387cbbfe7364e675abb5e4f2a5c806cd0fc7c
209 lines
5.7 KiB
C
209 lines
5.7 KiB
C
// 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;
|
|
}
|