commit aa048c556d77e44c3f7706b11a92268afd554cea Author: Brian Holman Date: Mon Nov 9 23:32:47 2026 -0700 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 diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..d1599d1 --- /dev/null +++ b/Makefile @@ -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) diff --git a/README.md b/README.md new file mode 100644 index 0000000..52c81a0 --- /dev/null +++ b/README.md @@ -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. diff --git a/astar-demo.c b/astar-demo.c new file mode 100644 index 0000000..c041df0 --- /dev/null +++ b/astar-demo.c @@ -0,0 +1,267 @@ +#include "astar.h" + +#include + + +#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; +} diff --git a/astar.c b/astar.c new file mode 100644 index 0000000..73b85c9 --- /dev/null +++ b/astar.c @@ -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; +} diff --git a/astar.h b/astar.h new file mode 100644 index 0000000..1f457d1 --- /dev/null +++ b/astar.h @@ -0,0 +1,72 @@ +#ifndef _astar_h +#define _astar_h + +#include + + +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 */