Import AStar 0.1.0 upstream source snapshot

Original author: Pierre Sarrazin. Public-domain package.
Source: http://sarrazip.com/dev/astar-0.1.0.tar.gz
Archive SHA-256: acd2d5e2ad709892cb7609f5e53387cbbfe7364e675abb5e4f2a5c806cd0fc7c
This commit is contained in:
Brian Holman committed 2026-11-09 23:32:47 -07:00
commit aa048c556d
5 files changed
+639

No files matched your search

+208
View File
@@ -0,0 +1,208 @@
// 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, &currentIndex, &currentX, &currentY))
{
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;
}