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
+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;
|
||||
}
|
||||
Reference in new issue
Block a user