#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; }