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,62 @@
|
|||||||
|
PACKAGE = astar
|
||||||
|
VERSION = 0.1.0
|
||||||
|
|
||||||
|
LIBSRC = astar.c
|
||||||
|
LIBHEADERS = astar.h
|
||||||
|
STATICLIB = lib$(PACKAGE).a
|
||||||
|
|
||||||
|
# Demo program.
|
||||||
|
DEMOSRC = astar-demo.c
|
||||||
|
DEMOBIN = astar.bin
|
||||||
|
|
||||||
|
DISTFILES = README.md Makefile $(DEMOSRC) $(LIBSRC) $(LIBHEADERS)
|
||||||
|
|
||||||
|
LIBOBJ = $(LIBSRC:.c=.o)
|
||||||
|
DEMOOBJ = $(DEMOSRC:.c=.o)
|
||||||
|
|
||||||
|
# Compilation and linking flags.
|
||||||
|
ORG = --org=2800
|
||||||
|
INTERMEDIATE = #--intermediate
|
||||||
|
CFLAGS = $(INTERMEDIATE)
|
||||||
|
LDFLAGS = $(ORG) $(INTERMEDIATE)
|
||||||
|
|
||||||
|
# Tools.
|
||||||
|
CMOC = cmoc
|
||||||
|
LWAR = lwar
|
||||||
|
TAR = tar
|
||||||
|
|
||||||
|
all: $(DEMOBIN)
|
||||||
|
|
||||||
|
$(DEMOBIN): $(DEMOOBJ) $(STATICLIB)
|
||||||
|
$(CMOC) -o $(DEMOBIN) $(LDFLAGS) $(DEMOOBJ) -L. -l$(PACKAGE)
|
||||||
|
|
||||||
|
$(STATICLIB): $(LIBOBJ)
|
||||||
|
$(LWAR) --create $(STATICLIB) $(LIBOBJ)
|
||||||
|
|
||||||
|
%.o: %.c
|
||||||
|
$(CMOC) -c $(CFLAGS) -o $@ $<
|
||||||
|
|
||||||
|
clean:
|
||||||
|
rm -f $(DEMOBIN) $(DEMOOBJ) $(STATICLIB) $(LIBOBJ)
|
||||||
|
|
||||||
|
.PHONY: dist distcheck
|
||||||
|
dist:
|
||||||
|
$(TAR) -czf $(PACKAGE)-$(VERSION).tar.gz --transform 's,^,$(PACKAGE)-$(VERSION)/,' $(DISTFILES)
|
||||||
|
|
||||||
|
distcheck: dist
|
||||||
|
mkdir ,distcheck
|
||||||
|
$(TAR) -C ,distcheck -xzf $(PACKAGE)-$(VERSION).tar.gz
|
||||||
|
$(MAKE) -C ,distcheck/$(PACKAGE)-$(VERSION) all
|
||||||
|
rm -fr ,distcheck
|
||||||
|
@echo "SUCCESS: $(PACKAGE)-$(VERSION).tar.gz ready for distribution"
|
||||||
|
|
||||||
|
# Automatic prerequisite generation.
|
||||||
|
%.d: %.c $(WORD_LENGTH_FILENAME)
|
||||||
|
$(CMOC) --deps-only $(CFLAGS) $<
|
||||||
|
|
||||||
|
.PHONY: cleandeps
|
||||||
|
cleandeps:
|
||||||
|
rm -f $(DEMOSRC:.c=.d) $(LIBSRC:.c=.d)
|
||||||
|
|
||||||
|
include $(DEMOSRC:.c=.d)
|
||||||
|
include $(LIBSRC:.c=.d)
|
||||||
@@ -0,0 +1,30 @@
|
|||||||
|
AStar: A demo of the A* search algorithm
|
||||||
|
========================================
|
||||||
|
|
||||||
|
By Pierre Sarrazin (http://sarrazip.com/).
|
||||||
|
|
||||||
|
CMOC Home Page: http://sarrazip.com/dev/cmoc.html
|
||||||
|
|
||||||
|
Version 0.1.0 - 202?-??-?? - First public release.
|
||||||
|
|
||||||
|
|
||||||
|
# Introduction
|
||||||
|
|
||||||
|
[A\*](https://en.wikipedia.org/wiki/A*_search_algorithm)
|
||||||
|
is a graph traversal and path search algorithm.
|
||||||
|
|
||||||
|
This package presents an implementation that assumes that the graph
|
||||||
|
is a rectangular grid.
|
||||||
|
|
||||||
|
See `astar-demo.c` for the demo. It uses `astar.h` and `astar.c`.
|
||||||
|
|
||||||
|
|
||||||
|
# Building the demo
|
||||||
|
|
||||||
|
After installing CMOC, type `make`, which should build the library
|
||||||
|
(`libastar.a`) and the demo executable (`astar.bin`).
|
||||||
|
|
||||||
|
|
||||||
|
# License
|
||||||
|
|
||||||
|
This package and library are in the public domain.
|
||||||
+267
@@ -0,0 +1,267 @@
|
|||||||
|
#include "astar.h"
|
||||||
|
|
||||||
|
#include <coco.h>
|
||||||
|
|
||||||
|
|
||||||
|
#define NUMELEMS(array) (sizeof(array) / sizeof((array)[0]))
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
A---B---C D
|
||||||
|
| | |
|
||||||
|
E F---G H
|
||||||
|
| |
|
||||||
|
I---J---K---L
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct UserNode
|
||||||
|
{
|
||||||
|
const char *name;
|
||||||
|
byte column, row; // position in the grid, in terms of nodes
|
||||||
|
} UserNode;
|
||||||
|
|
||||||
|
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
WIDTH = 4u,
|
||||||
|
HEIGHT = 3u,
|
||||||
|
NUM_USER_NODES = WIDTH * HEIGHT,
|
||||||
|
|
||||||
|
// Display.
|
||||||
|
SCREEN_BG_COLOR = 3, // dark blue
|
||||||
|
EDGE_COLOR = 5, // white
|
||||||
|
GRID_TOP_ROW = 2,
|
||||||
|
GRID_LEFT_COL = 8,
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
static UserNode userNodes[NUM_USER_NODES] =
|
||||||
|
{
|
||||||
|
{ "A", 0, 0 },
|
||||||
|
{ "B", 1, 0 },
|
||||||
|
{ "C", 2, 0 },
|
||||||
|
{ "D", 3, 0 },
|
||||||
|
{ "E", 0, 1 },
|
||||||
|
{ "F", 1, 1 },
|
||||||
|
{ "G", 2, 1 },
|
||||||
|
{ "H", 3, 1 },
|
||||||
|
{ "I", 0, 2 },
|
||||||
|
{ "J", 1, 2 },
|
||||||
|
{ "K", 2, 2 },
|
||||||
|
{ "L", 3, 2 },
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct NodeNeighborhood { BOOL canGoEast, canGoSouth; } NodeNeighborhood;
|
||||||
|
|
||||||
|
|
||||||
|
static NodeNeighborhood nodeNeighborhood[NUM_USER_NODES] =
|
||||||
|
{
|
||||||
|
{ 1, 1 }, // A
|
||||||
|
{ 1, 0 }, // B
|
||||||
|
{ 0, 1 }, // C
|
||||||
|
{ 0, 1 }, // D
|
||||||
|
{ 0, 0 }, // E
|
||||||
|
{ 1, 1 }, // F
|
||||||
|
{ 0, 0 }, // G
|
||||||
|
{ 0, 1 }, // H
|
||||||
|
{ 1, 0 }, // I
|
||||||
|
{ 1, 0 }, // J
|
||||||
|
{ 1, 0 }, // K
|
||||||
|
{ 0, 0 }, // L
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
static AStarNode astarNodes[NUM_USER_NODES];
|
||||||
|
|
||||||
|
|
||||||
|
void
|
||||||
|
sleepTicks(size_t numTicks)
|
||||||
|
{
|
||||||
|
for (size_t limit = getTimer() + numTicks; (int) (getTimer() - limit) <= 0; )
|
||||||
|
;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Draws a 3x3 square on the text screen.
|
||||||
|
//
|
||||||
|
void
|
||||||
|
drawNode(const UserNode *userNode, const NodeNeighborhood *neighborhood)
|
||||||
|
{
|
||||||
|
byte nodeCol = GRID_LEFT_COL + userNode->column * 4;
|
||||||
|
byte topRow = GRID_TOP_ROW + userNode->row * 4;
|
||||||
|
for (byte row = 0; row < 3; ++row)
|
||||||
|
{
|
||||||
|
locate(nodeCol, topRow + row);
|
||||||
|
printf("\xEF\xEF\xEF"); // magenta blocks
|
||||||
|
}
|
||||||
|
locate(nodeCol, topRow);
|
||||||
|
printf("%s", userNode->name); // single char name expected
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void
|
||||||
|
drawNeighborhood(const UserNode *userNode, const NodeNeighborhood *neighborhood)
|
||||||
|
{
|
||||||
|
byte nodeCol = GRID_LEFT_COL + userNode->column * 4 + 1;
|
||||||
|
byte nodeRow = GRID_TOP_ROW + userNode->row * 4 + 1;
|
||||||
|
if (neighborhood->canGoEast)
|
||||||
|
{
|
||||||
|
locate(nodeCol, nodeRow);
|
||||||
|
printf("\x80\x80\x80\x80\x80"); // black blocks
|
||||||
|
}
|
||||||
|
if (neighborhood->canGoSouth)
|
||||||
|
for (byte row = 0; row < 5; ++row)
|
||||||
|
{
|
||||||
|
locate(nodeCol, nodeRow + row);
|
||||||
|
putchar('\x80');
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Gets the increment to be added to 'a' to progress towards 'b'.
|
||||||
|
//
|
||||||
|
char
|
||||||
|
getIncrement(byte src, byte dst)
|
||||||
|
{
|
||||||
|
return src < dst ? +1 : (src > dst ? -1 : 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// x: 0..63.
|
||||||
|
// y: 0..31.
|
||||||
|
// color: 1..8.
|
||||||
|
//
|
||||||
|
void
|
||||||
|
setPixelSG4(byte x, byte y, byte color)
|
||||||
|
{
|
||||||
|
byte *byteToChange = (byte *) 0x0400 + ((word) (y >> 1) << 5) + (x >> 1);
|
||||||
|
byte b = *byteToChange & 0x8F; // read screen byte but ignore existing color bits
|
||||||
|
if ((b & 0x80) == 0) // if not already semi-graphics char
|
||||||
|
b = 0;
|
||||||
|
byte setBit = 8;
|
||||||
|
if (x & 1) // if horiz. coord. is odd
|
||||||
|
setBit >>= 1;
|
||||||
|
if (y & 1) // if vert. coord. is odd
|
||||||
|
setBit >>= 2;
|
||||||
|
*byteToChange = b | setBit | ((color - 1) << 4);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void
|
||||||
|
drawEdge(const UserNode *srcNode, const UserNode *dstNode)
|
||||||
|
{
|
||||||
|
// Get positions on 64x32 semi-graphics screen.
|
||||||
|
byte srcNodeCol = 2 * (GRID_LEFT_COL + srcNode->column * 4 + 1) + 1;
|
||||||
|
byte srcNodeRow = 2 * (GRID_TOP_ROW + srcNode->row * 4 + 1) + 1;
|
||||||
|
byte dstNodeCol = 2 * (GRID_LEFT_COL + dstNode->column * 4 + 1) + 1;
|
||||||
|
byte dstNodeRow = 2 * (GRID_TOP_ROW + dstNode->row * 4 + 1) + 1;
|
||||||
|
|
||||||
|
// Get a (dx, dy) vector of the edge's orientation.
|
||||||
|
char dx = getIncrement(srcNodeCol, dstNodeCol);
|
||||||
|
char dy = getIncrement(srcNodeRow, dstNodeRow);
|
||||||
|
|
||||||
|
for (byte i = 0; i < 9; ++i) // number of pixels per edge
|
||||||
|
{
|
||||||
|
setPixelSG4(srcNodeCol, srcNodeRow, EDGE_COLOR);
|
||||||
|
srcNodeCol += (byte) dx;
|
||||||
|
srcNodeRow += (byte) dy;
|
||||||
|
sleepTicks(5);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
size_t
|
||||||
|
countFoundPathLength(AStarGraph *graph, size_t goalX, size_t goalY)
|
||||||
|
{
|
||||||
|
AStarPathIterator iter;
|
||||||
|
astar_startPathIteration(&iter, graph, goalX, goalY);
|
||||||
|
AStarNode *node;
|
||||||
|
size_t length = 0;
|
||||||
|
while (node = astar_advancePathIteration(&iter))
|
||||||
|
++length;
|
||||||
|
return length;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int
|
||||||
|
main()
|
||||||
|
{
|
||||||
|
initCoCoSupport();
|
||||||
|
cls(SCREEN_BG_COLOR);
|
||||||
|
|
||||||
|
locate(12, 0);
|
||||||
|
printf("A* DEMO");
|
||||||
|
|
||||||
|
// Initialize the nodes. This puts a UserNode address in each AStarNode object.
|
||||||
|
// Note that the astar library does not known about struct UserNode, which is
|
||||||
|
// private to this demo program.
|
||||||
|
//
|
||||||
|
for (byte i = 0; i < NUM_USER_NODES; ++i)
|
||||||
|
{
|
||||||
|
astar_initNode(&astarNodes[i],
|
||||||
|
&userNodes[i],
|
||||||
|
nodeNeighborhood[i].canGoEast,
|
||||||
|
nodeNeighborhood[i].canGoSouth);
|
||||||
|
drawNode(&userNodes[i], &nodeNeighborhood[i]);
|
||||||
|
}
|
||||||
|
for (byte i = 0; i < NUM_USER_NODES; ++i)
|
||||||
|
drawNeighborhood(&userNodes[i], &nodeNeighborhood[i]);
|
||||||
|
|
||||||
|
// At this point, nodeNeighborhood[] is not needed anymore.
|
||||||
|
// Its information is now in astarNodes[].
|
||||||
|
// If we were short on memory, we could reuse the space occupied by nodeNeighborhood[].
|
||||||
|
|
||||||
|
// Search the grid for a path from node A ((0, 0), i.e., top left) to node L (bottom right).
|
||||||
|
//
|
||||||
|
AStarGraph astarGraph = { &astarNodes, WIDTH, HEIGHT };
|
||||||
|
size_t goalX = WIDTH - 1, goalY = HEIGHT - 1; // L
|
||||||
|
BOOL foundPath = astar_searchGrid(&astarGraph, 0, 0, goalX, goalY);
|
||||||
|
|
||||||
|
if (foundPath)
|
||||||
|
{
|
||||||
|
locate(2, 14);
|
||||||
|
printf("FOUND PATH:");
|
||||||
|
|
||||||
|
UserNode *foundPathNodes[8];
|
||||||
|
size_t pathLen = countFoundPathLength(&astarGraph, goalX, goalY);
|
||||||
|
if (pathLen > NUMELEMS(foundPathNodes)) // if foundPathNodes[] too short
|
||||||
|
printf("TOO LONG.");
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Collect the list of UserNodes that are part of the path,
|
||||||
|
// in reverse order than how they are provided by astar_advancePathIteration().
|
||||||
|
// foundPathNodes[] will thus contain the path going from the start to the goal.
|
||||||
|
//
|
||||||
|
AStarPathIterator iter;
|
||||||
|
astar_startPathIteration(&iter, &astarGraph, goalX, goalY);
|
||||||
|
size_t index = pathLen; // used to fill foundPathNodes[] from right to left
|
||||||
|
AStarNode *node;
|
||||||
|
while (node = astar_advancePathIteration(&iter))
|
||||||
|
foundPathNodes[--index] = (UserNode *) node->userData;
|
||||||
|
|
||||||
|
// Print the node names from start to goal.
|
||||||
|
//
|
||||||
|
for ( ; index < pathLen; ++index)
|
||||||
|
printf(" %s", foundPathNodes[index]->name);
|
||||||
|
|
||||||
|
sleepTicks(60);
|
||||||
|
|
||||||
|
// Draw the edges of the path.
|
||||||
|
//
|
||||||
|
for (index = 1; index < pathLen; ++index)
|
||||||
|
drawEdge(foundPathNodes[index - 1], foundPathNodes[index]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
locate(6, 14);
|
||||||
|
printf("FAILED TO FIND PATH.");
|
||||||
|
}
|
||||||
|
|
||||||
|
waitkey(FALSE);
|
||||||
|
cls(255);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,72 @@
|
|||||||
|
#ifndef _astar_h
|
||||||
|
#define _astar_h
|
||||||
|
|
||||||
|
#include <coco.h>
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct AStarNode
|
||||||
|
{
|
||||||
|
void *userData; // can be used to point to more data about this node
|
||||||
|
byte searchSet; // enum SearchSet
|
||||||
|
|
||||||
|
// These describe the relation of this node with the grid of nodes that it belongs to.
|
||||||
|
BOOL canGoEast;
|
||||||
|
BOOL canGoSouth;
|
||||||
|
|
||||||
|
// Used by the A* algorithm.
|
||||||
|
word gScore;
|
||||||
|
word fScore;
|
||||||
|
struct AStarNode *cameFrom; // node that precedes this one in a path
|
||||||
|
} AStarNode;
|
||||||
|
|
||||||
|
|
||||||
|
// Represents a grid of nodes to be searched.
|
||||||
|
//
|
||||||
|
typedef struct AStarGraph
|
||||||
|
{
|
||||||
|
AStarNode *nodes; // row-major matrix of nodes
|
||||||
|
size_t width; // number of matrix columns
|
||||||
|
size_t height; // number of matrix rows
|
||||||
|
} AStarGraph;
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct AStarPathIterator
|
||||||
|
{
|
||||||
|
AStarGraph *graph;
|
||||||
|
AStarNode *node;
|
||||||
|
} AStarPathIterator;
|
||||||
|
|
||||||
|
|
||||||
|
void astar_initNode(AStarNode *node,
|
||||||
|
void *userData,
|
||||||
|
byte canGoEast, byte canGoSouth);
|
||||||
|
|
||||||
|
|
||||||
|
// Returns non-zero if a path is found from (startX, startY) to (goalX, goalY).
|
||||||
|
// In this case, astar_startPathIteration() can be called to obtain this path.
|
||||||
|
// Returns zero if no path is found.
|
||||||
|
//
|
||||||
|
BOOL astar_searchGrid(AStarGraph *graph,
|
||||||
|
size_t startX, size_t startY,
|
||||||
|
size_t goalX, size_t goalY);
|
||||||
|
|
||||||
|
|
||||||
|
// Starts an iteration through the graph that goes from the specified goal position
|
||||||
|
// to the starting node specified to the most recent call to astar_searchGrid().
|
||||||
|
//
|
||||||
|
// goalX, goalY: Must be the goal position found by a successful call to astar_searchGrid().
|
||||||
|
// After calling this, call astar_advancePathIteration() until it returns NULL.
|
||||||
|
//
|
||||||
|
void astar_startPathIteration(AStarPathIterator *iter,
|
||||||
|
AStarGraph *graph,
|
||||||
|
size_t goalX, size_t goalY);
|
||||||
|
|
||||||
|
|
||||||
|
// Returns the address of the next node in the iteration,
|
||||||
|
// or NULL if the iteration is at the end.
|
||||||
|
// *iter must have been initialized by astar_startPathIteration().
|
||||||
|
//
|
||||||
|
AStarNode *astar_advancePathIteration(AStarPathIterator *iter);
|
||||||
|
|
||||||
|
|
||||||
|
#endif /* _astar_h */
|
||||||
Reference in new issue
Block a user