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:
commit
aa048c556d
5 files changed
+639
No files matched your search
@@ -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, ¤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;
|
||||
}
|
||||
Reference in new issue
Block a user