--- src/astar.c 2010-07-02 17:54:56.000000000 +0200 +++ src/astar.cpp 2010-07-02 17:48:12.000000000 +0200 @@ -1,7 +1,7 @@ /* This file is part of Warzone 2100. Copyright (C) 1999-2004 Eidos Interactive - Copyright (C) 2005-2009 Warzone Resurrection Project + Copyright (C) 2005-2010 Warzone Resurrection Project Warzone 2100 is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -28,33 +28,54 @@ #include "map.h" #endif +#include +#include + /** Counter to implement lazy deletion from nodeArray. * * @see fpathTableReset */ static uint16_t resetIterationCount = 0; +/// A coordinate. +struct PathCoord +{ + PathCoord() {} + PathCoord(int16_t x_, int16_t y_) : x(x_), y(y_) {} + bool operator ==(PathCoord const &z) const { return x == z.x && y == z.y; } + bool operator !=(PathCoord const &z) const { return !(*this == z); } + + int16_t x, y; +}; + /** The structure to store a node of the route in node table * * @ingroup pathfinding */ -typedef struct _fp_node +struct PathNode { - struct _fp_node *psOpen; - struct _fp_node *psRoute; // Previous point in the route - uint16_t iteration; - short x, y; // map coords - short dist, est; // distance so far and estimate to end - short type; // open or closed node -} FP_NODE; - -// types of node -#define NT_OPEN 1 -#define NT_CLOSED 2 + bool operator <(PathNode const &z) const + { + // Sort decending est, fallback to ascending dist, fallback to sorting by position. + return est > z.est || + (est == z.est && + dist < z.dist || + (dist == z.dist && + p.x < z.p.x || + (p.x == z.p.x && + p.y < z.p.y))); + } -/** List of open nodes - */ -static FP_NODE* psOpen; + PathCoord p; // Map coords. + unsigned dist, est; // Distance so far and estimate to end. +}; +struct PathExploredNode +{ + uint16_t iteration; + int8_t dx, dy; // Offset from previous point in the route. + unsigned dist; // Shortest known distance to tile. + bool visited; +}; /** A map's maximum width & height */ @@ -62,13 +83,11 @@ /** Table of closed nodes */ -static FP_NODE* nodeArray[MAX_MAP_SIZE][MAX_MAP_SIZE] = { { NULL }, { NULL } }; - -#define NUM_DIR 8 +static PathExploredNode nodeArray[MAX_MAP_SIZE][MAX_MAP_SIZE]; // Convert a direction into an offset // dir 0 => x = 0, y = -1 -static const Vector2i aDirOffset[NUM_DIR] = +static const Vector2i aDirOffset[] = { { 0, 1}, {-1, 1}, @@ -80,56 +99,6 @@ { 1, 1}, }; -/** Add a node to the node table - * - * @param psNode to add to the table - */ -static void fpathAddNode(FP_NODE* psNode) -{ - const int x = psNode->x; - const int y = psNode->y; - - ASSERT(x < ARRAY_SIZE(nodeArray) && y < ARRAY_SIZE(nodeArray[x]), "X (%d) or Y %d) coordinate for path finding node is out of range!", x, y); - - // Lets not leak memory - if (nodeArray[x][y]) - { - free(nodeArray[x][y]); - } - - nodeArray[x][y] = psNode; - - // Assign this node to the current iteration (this node will only remain - // valid for as long as it's `iteration` member has the same value as - // resetIterationCount. - psNode->iteration = resetIterationCount; -} - -/** See if a node is in the table - * Check whether there is a node for the given coordinates in the table - * - * @param x,y the coordinates to check for - * @return a pointer to the node if one could be found, or NULL otherwise. - */ -static FP_NODE* fpathGetNode(int x, int y) -{ - FP_NODE * psFound; - - if (x < 0 || y < 0 || x >= ARRAY_SIZE(nodeArray) || y >= ARRAY_SIZE(nodeArray[x])) - { - return NULL; - } - - psFound = nodeArray[x][y]; - if (psFound - && psFound->iteration == resetIterationCount) - { - return psFound; - } - - return NULL; -} - /** Reset the node table * * @NOTE The actual implementation does a lazy reset, because resetting the @@ -143,7 +112,7 @@ ++resetIterationCount; // Check to prevent overflows of resetIterationCount - if (resetIterationCount < UINT16_MAX - 1) + if (resetIterationCount < UINT16_MAX) { ASSERT(resetIterationCount > 0, "Integer overflow occurred!"); @@ -164,152 +133,115 @@ { for (y = 0; y < ARRAY_SIZE(nodeArray[x]); ++y) { - if (nodeArray[x][y]) - { - free(nodeArray[x][y]); - nodeArray[x][y] = NULL; - } + nodeArray[x][y].iteration = UINT16_MAX; } } resetIterationCount = 0; } -/** Add a node to the open list - */ -static inline void fpathOpenAdd(FP_NODE *psNode) -{ - psNode->psOpen = psOpen; - psOpen = psNode; -} - /** Get the nearest entry in the open list */ -static FP_NODE *fpathOpenGet(void) +/// Takes the current best node, and removes from the node tree. +static inline PathNode fpathTakeNode(std::vector &nodes) { - FP_NODE *psNode, *psCurr, *psPrev, *psParent = NULL; - - if (psOpen == NULL) - { - return NULL; - } - // find the node with the lowest distance - psPrev = NULL; - psNode = psOpen; - for(psCurr = psOpen; psCurr; psCurr = psCurr->psOpen) - { - const int first = psCurr->dist + psCurr->est; - const int second = psNode->dist + psNode->est; - - // if equal totals, give preference to node closer to target - if (first < second || (first == second && psCurr->est < psNode->est)) - { - psParent = psPrev; - psNode = psCurr; - } - - psPrev = psCurr; - } + // if equal totals, give preference to node closer to target + PathNode ret = nodes.front(); // remove the node from the list - if (psNode == psOpen) - { - // node is at head of list - psOpen = psOpen->psOpen; - } - else - { - psParent->psOpen = psNode->psOpen; - } + std::pop_heap(nodes.begin(), nodes.end()); + nodes.pop_back(); - return psNode; + return ret; } /** Estimate the distance to the target point */ -static SDWORD WZ_DECL_CONST fpathEstimate(SDWORD x, SDWORD y, SDWORD fx, SDWORD fy) +static inline unsigned WZ_DECL_CONST fpathEstimate(PathCoord s, PathCoord f) { - SDWORD xdiff, ydiff; - - xdiff = x > fx ? x - fx : fx - x; - ydiff = y > fy ? y - fy : fy - y; - - xdiff = xdiff * 10; - ydiff = ydiff * 10; - - return xdiff > ydiff ? xdiff + ydiff/2 : xdiff/2 + ydiff; + // Cost of moving horizontal/vertical = 70, cost of moving diagonal = 99, 99/70 = 1.41428571... ≈ √2 = 1.41421356... + unsigned xDelta = abs(s.x - f.x), yDelta = abs(s.y - f.y); + return std::min(xDelta, yDelta)*(99 - 70) + std::max(xDelta, yDelta)*70; } /** Generate a new node */ -static FP_NODE *fpathNewNode(SDWORD x, SDWORD y, SDWORD dist, FP_NODE *psRoute) +static inline void fpathNewNode(std::vector &nodes, PathCoord dest, PathCoord pos, unsigned prevDist, PathCoord prevPos) { - FP_NODE *psNode = malloc(sizeof(FP_NODE)); + ASSERT(pos.x < ARRAY_SIZE(nodeArray) && pos.y < ARRAY_SIZE(nodeArray[pos.x]), "X (%d) or Y (%d) coordinate for path finding node is out of range!", pos.x, pos.y); - if (psNode == NULL) + // Create the node. + PathNode node; + node.p = pos; + node.dist = prevDist + fpathEstimate(prevPos, pos); + node.est = node.dist + fpathEstimate(pos, dest); + + PathExploredNode &expl = nodeArray[pos.x][pos.y]; + if (expl.iteration == resetIterationCount && (expl.visited || expl.dist <= node.dist)) { - debug(LOG_ERROR, "fpathNewNode: Out of memory"); - return NULL; + return; // Already visited this tile. Do nothing. } - psNode->x = (SWORD)x; - psNode->y = (SWORD)y; - psNode->dist = (SWORD)dist; - psNode->psRoute = psRoute; - psNode->type = NT_OPEN; + // Remember where we have been, and remember the way back. + expl.iteration = resetIterationCount; + expl.dx = pos.x - prevPos.x; + expl.dy = pos.y - prevPos.y; + expl.dist = node.dist; + expl.visited = false; - return psNode; + // Add the node to the node tree. + nodes.push_back(node); + std::push_heap(nodes.begin(), nodes.end()); } +static std::vector nodes; +static std::vector path; + SDWORD fpathAStarRoute(MOVE_CONTROL *psMove, PATHJOB *psJob) { - FP_NODE *psFound, *psCurr, *psNew; - FP_NODE *psNearest, *psRoute; - SDWORD dir, x, y, currDist; - SDWORD retval = ASR_OK; - const int tileSX = map_coord(psJob->origX); - const int tileSY = map_coord(psJob->origY); - const int tileFX = map_coord(psJob->destX); - const int tileFY = map_coord(psJob->destY); + PathCoord nearestCoord(0, 0); + unsigned nearestDist = 0xFFFFFFFF; + +// SDWORD dir, x, y, currDist; + int retval = ASR_OK; + + const PathCoord tileS(map_coord(psJob->origX), map_coord(psJob->origY)); + const PathCoord tileF(map_coord(psJob->destX), map_coord(psJob->destY)); fpathTableReset(); + nodes.clear(); // Could declare nodes here, but making global to save allocations. // Add the start point to the open list - psCurr = fpathNewNode(tileSX,tileSY, 0, NULL); - if (!psCurr) - { - fpathTableReset(); - return ASR_FAILED; - } - // estimate the estimated distance/moves - psCurr->est = (SWORD)fpathEstimate(psCurr->x, psCurr->y, tileFX, tileFY); - psOpen = NULL; - fpathOpenAdd(psCurr); - fpathAddNode(psCurr); - psRoute = NULL; - psNearest = NULL; + fpathNewNode(nodes, tileF, tileS, 0, tileS); + ASSERT(!nodes.empty(), "fpathNewNode failed to add node."); // search for a route - while (psOpen != NULL) + while (!nodes.empty()) { - psCurr = fpathOpenGet(); - - if (psCurr->x == tileFX && psCurr->y == tileFY) + PathNode node = fpathTakeNode(nodes); + if (nodeArray[node.p.x][node.p.y].visited) { - // reached the target - psRoute = psCurr; - break; + continue; // Already been here. } + nodeArray[node.p.x][node.p.y].visited = true; // note the nearest node to the target so far - if (psNearest == NULL || psCurr->est < psNearest->est) + if (node.est - node.dist < nearestDist) + { + nearestCoord = node.p; + nearestDist = node.est - node.dist; + } + + if (node.p == tileF) { - psNearest = psCurr; + // reached the target + nearestCoord = node.p; + break; } // loop through possible moves in 8 directions to find a valid move - for (dir = 0; dir < NUM_DIR; dir += 1) + for (int dir = 0; dir < ARRAY_SIZE(aDirOffset); ++dir) { /* make non-orthogonal-adjacent moves' dist a bit longer/cost a bit more 5 6 7 @@ -318,151 +250,98 @@ /|\ 3 2 1 odd:orthogonal-adjacent tiles even:non-orthogonal-adjacent tiles - odd ones get extra 4 units(1.414 times) of distance/costs */ - if (dir % 2 == 0) - { - currDist = psCurr->dist + 10; - } - else + if (dir % 2 != 0) { + int x, y; + // We cannot cut corners - x = psCurr->x + aDirOffset[(dir + 1) % 8].x; - y = psCurr->y + aDirOffset[(dir + 1) % 8].y; + x = node.p.x + aDirOffset[(dir + 1) % 8].x; + y = node.p.y + aDirOffset[(dir + 1) % 8].y; if (fpathBaseBlockingTile(x, y, psJob->propulsion, psJob->owner, psJob->moveType)) { continue; } - x = psCurr->x + aDirOffset[(dir - 1) % 8].x; - y = psCurr->y + aDirOffset[(dir - 1) % 8].y; + x = node.p.x + aDirOffset[(dir + 7) % 8].x; + y = node.p.y + aDirOffset[(dir + 7) % 8].y; if (fpathBaseBlockingTile(x, y, psJob->propulsion, psJob->owner, psJob->moveType)) { continue; } - - currDist = psCurr->dist + 14; } // Try a new location - x = psCurr->x + aDirOffset[dir].x; - y = psCurr->y + aDirOffset[dir].y; + int x = node.p.x + aDirOffset[dir].x; + int y = node.p.y + aDirOffset[dir].y; - // See if the node has already been visited - psFound = fpathGetNode(x, y); - if (psFound && psFound->dist <= currDist) - { - // already visited node by a shorter route - continue; - } - - // If the tile hasn't been visited see if it is a blocking tile - if (!psFound && fpathBaseBlockingTile(x, y, psJob->propulsion, psJob->owner, psJob->moveType)) + // See if the node is a blocking tile + if (fpathBaseBlockingTile(x, y, psJob->propulsion, psJob->owner, psJob->moveType)) { // tile is blocked, skip it continue; } - // Now insert the point into the appropriate list - if (!psFound) - { - // Not in open or closed lists - add to the open list - psNew = fpathNewNode(x,y, currDist, psCurr); - if (psNew) - { - psNew->est = fpathEstimate(x, y, tileFX, tileFY); - fpathOpenAdd(psNew); - fpathAddNode(psNew); - } - } - else if (psFound->type == NT_OPEN) - { - // already in the open list but this is shorter - psFound->dist = (SWORD)currDist; - psFound->psRoute = psCurr; - } - else if (psFound->type == NT_CLOSED) - { - // already in the closed list but this is shorter - psFound->type = NT_OPEN; - psFound->dist = (SWORD)currDist; - psFound->psRoute = psCurr; - fpathOpenAdd(psFound); - } + // Now insert the point into the appropriate list, if not already visited. + fpathNewNode(nodes, tileF, PathCoord(x, y), node.dist, node.p); } - - // add the current point to the closed nodes - psCurr->type = NT_CLOSED; } + ASSERT(nearestDist != 0xFFFFFFFF, "Failed to find a point closer to the destination than infinity."); // return the nearest route if no actual route was found - if (!psRoute && psNearest) + if (nearestCoord != tileF) { - psRoute = psNearest; retval = ASR_NEAREST; } - if (psRoute) + // Get route, in reverse order. + path.clear(); // Could declare path here, but making global to save allocations. + for (PathCoord p = nearestCoord; p != tileS; p = PathCoord(p.x - nodeArray[p.x][p.y].dx, p.y - nodeArray[p.x][p.y].dy)) { - int count = 0; // calculated length of route + ASSERT(tileOnMap(p.x, p.y), "Assigned XY coordinates (%d, %d) not on map!", (int)p.x, (int)p.y); - // Count length of route - psCurr = psRoute; - while (psCurr) - { - ASSERT(tileOnMap(psCurr->x, psCurr->y), "Assigned XY coordinates (%d, %d) not on map!", (int)psCurr->x, (int)psCurr->y); - psCurr = psCurr->psRoute; - count++; - } + Vector2i v = {world_coord(p.x) + TILE_UNITS / 2, world_coord(p.y) + TILE_UNITS / 2}; + path.push_back(v); + } + if (path.empty()) + { + // We are probably already in the destination tile. Go to the exact coordinates. + Vector2i v = {psJob->destX, psJob->destY}; + path.push_back(v); + } + else if (retval == ASR_OK) + { + // Found exact path, so use exact coordinates for last point, no reason to lose precision + Vector2i v = {psJob->destX, psJob->destY}; + path.front() = v; + } - // TODO FIXME once we can change numPoints to something larger than uchar - psMove->numPoints = MIN(255, count); + // TODO FIXME once we can change numPoints to something larger than uint16_t + psMove->numPoints = std::min(UINT16_MAX, path.size()); - // Allocate memory - psMove->asPath = calloc(sizeof(*psMove->asPath), count); - ASSERT(psMove->asPath, "Out of memory"); - if (!psMove->asPath) - { - fpathHardTableReset(); - return ASR_FAILED; - } - - // get the route in the correct order - // If as I suspect this is to reverse the list, then it's my suspicion that - // we could route from destination to source as opposed to source to - // destination. We could then save the reversal. to risky to try now...Alex M - // - // The idea is impractical, because you can't guarentee that the target is - // reachable. As I see it, this is the reason why psNearest got introduced. - // -- Dennis L. - psCurr = psRoute; - if (psCurr && retval == ASR_OK) - { - // Found exact path, so use exact coordinates for last point, no reason to lose precision - count--; - psMove->asPath[count].x = psJob->destX; - psMove->asPath[count].y = psJob->destY; - psMove->DestinationX = psJob->destX; - psMove->DestinationY = psJob->destY; - psCurr = psCurr->psRoute; - } - else - { - psMove->DestinationX = world_coord(psCurr->x) + TILE_UNITS / 2; - psMove->DestinationY = world_coord(psCurr->y) + TILE_UNITS / 2; - } - while (psCurr) - { - count--; - psMove->asPath[count].x = world_coord(psCurr->x) + TILE_UNITS / 2; - psMove->asPath[count].y = world_coord(psCurr->y) + TILE_UNITS / 2; - psCurr = psCurr->psRoute; - } - } - else + // Allocate memory + psMove->asPath = static_cast(malloc(sizeof(*psMove->asPath) * path.size())); + ASSERT(psMove->asPath, "Out of memory"); + if (!psMove->asPath) { - retval = ASR_FAILED; + fpathHardTableReset(); + return ASR_FAILED; } - fpathTableReset(); + // get the route in the correct order + // If as I suspect this is to reverse the list, then it's my suspicion that + // we could route from destination to source as opposed to source to + // destination. We could then save the reversal. to risky to try now...Alex M + // + // The idea is impractical, because you can't guarentee that the target is + // reachable. As I see it, this is the reason why psNearest got introduced. + // -- Dennis L. + // + // If many droids are heading towards the same destination, then destination + // to source would be faster if reusing the information in nodeArray. --Cyp + std::copy(path.rbegin(), path.rend(), psMove->asPath); + + psMove->DestinationX = path.back().x; + psMove->DestinationY = path.back().y; + return retval; }