Path Map::findPath(int startX, int startY, int destX, int destY, unsigned char walkmask, int maxCost) { // Path to be built up (empty by default) Path path; // Declare open list, a list with open tiles sorted on F cost std::priority_queue<Location> openList; // Return when destination not walkable if (!getWalk(destX, destY, walkmask)) return path; // Reset starting tile's G cost to 0 MetaTile *startTile = getMetaTile(startX, startY); startTile->Gcost = 0; // Add the start point to the open list openList.push(Location(startX, startY, startTile)); bool foundPath = false; // Keep trying new open tiles until no more tiles to try or target found while (!openList.empty() && !foundPath) { // Take the location with the lowest F cost from the open list. Location curr = openList.top(); openList.pop(); // If the tile is already on the closed list, this means it has already // been processed with a shorter path to the start point (lower G cost) if (curr.tile->whichList == mOnClosedList) { continue; } // Put the current tile on the closed list curr.tile->whichList = mOnClosedList; // Check the adjacent tiles for (int dy = -1; dy <= 1; dy++) { for (int dx = -1; dx <= 1; dx++) { // Calculate location of tile to check const int x = curr.x + dx; const int y = curr.y + dy; // Skip if if we're checking the same tile we're leaving from, // or if the new location falls outside of the map boundaries if ((dx == 0 && dy == 0) || !contains(x, y)) { continue; } MetaTile *newTile = getMetaTile(x, y); // Skip if the tile is on the closed list or is not walkable // unless its the destination tile if (newTile->whichList == mOnClosedList || ((newTile->blockmask & walkmask) && !(x == destX && y == destY))) { continue; } // When taking a diagonal step, verify that we can skip the // corner. if (dx != 0 && dy != 0) { MetaTile *t1 = getMetaTile(curr.x, curr.y + dy); MetaTile *t2 = getMetaTile(curr.x + dx, curr.y); if ((t1->blockmask | t2->blockmask) & BLOCKMASK_WALL) continue; } // Calculate G cost for this route, ~sqrt(2) for moving diagonal int Gcost = curr.tile->Gcost + (dx == 0 || dy == 0 ? basicCost : basicCost * 362 / 256); /* Demote an arbitrary direction to speed pathfinding by adding a defect (TODO: change depending on the desired visual effect, e.g. a cross-product defect toward destination). Important: as long as the total defect along any path is less than the basicCost, the pathfinder will still find one of the shortest paths! */ if (dx == 0 || dy == 0) { // Demote horizontal and vertical directions, so that two // consecutive directions cannot have the same Fcost. ++Gcost; } // It costs extra to walk through a being (needs to be enough // to make it more attractive to walk around). if (occupied(x, y)) { Gcost += 3 * basicCost; } // Skip if Gcost becomes too much // Warning: probably not entirely accurate if (Gcost > maxCost * basicCost) { continue; } if (newTile->whichList != mOnOpenList) { // Found a new tile (not on open nor on closed list) /* Update Hcost of the new tile. The pathfinder does not work reliably if the heuristic cost is higher than the real cost. In particular, using Manhattan distance is forbidden here. */ int dx = std::abs(x - destX), dy = std::abs(y - destY); newTile->Hcost = std::abs(dx - dy) * basicCost + std::min(dx, dy) * (basicCost * 362 / 256); // Set the current tile as the parent of the new tile newTile->parentX = curr.x; newTile->parentY = curr.y; // Update Gcost and Fcost of new tile newTile->Gcost = Gcost; newTile->Fcost = Gcost + newTile->Hcost; if (x != destX || y != destY) { // Add this tile to the open list newTile->whichList = mOnOpenList; openList.push(Location(x, y, newTile)); } else { // Target location was found foundPath = true; } } else if (Gcost < newTile->Gcost) { // Found a shorter route. // Update Gcost and Fcost of the new tile newTile->Gcost = Gcost; newTile->Fcost = Gcost + newTile->Hcost; // Set the current tile as the parent of the new tile newTile->parentX = curr.x; newTile->parentY = curr.y; // Add this tile to the open list (it's already // there, but this instance has a lower F score) openList.push(Location(x, y, newTile)); } } } } // Two new values to indicate whether a tile is on the open or closed list, // this way we don't have to clear all the values between each pathfinding. mOnClosedList += 2; mOnOpenList += 2; // If a path has been found, iterate backwards using the parent locations // to extract it. if (foundPath) { int pathX = destX; int pathY = destY; while (pathX != startX || pathY != startY) { // Add the new path node to the start of the path list path.push_front(Position(pathX, pathY)); // Find out the next parent MetaTile *tile = getMetaTile(pathX, pathY); pathX = tile->parentX; pathY = tile->parentY; } } return path; }
Path Map::findPath(const int startX, const int startY, const int destX, const int destY) { // Path to be built up (empty by default) Path path; // Declare open list, a list with open tiles sorted on F cost std::priority_queue<Location> openList; // Reset starting tile's G cost to 0 MetaTile *startTile = getMetaTile(startX, startY); startTile->Gcost = 0; // Add the start point to the open list openList.push(Location(startX, startY, startTile)); bool foundPath = false; // Keep trying new open tiles until no more tiles to try or target found while (!openList.empty() && !foundPath) { // Take the location with the lowest F cost from the open list. Location curr = openList.top(); openList.pop(); // If the tile is already on the closed list, this means it has already // been processed with a shorter path to the start point (lower G cost) if (curr.tile->whichList == mOnClosedList) continue; // Put the current tile on the closed list curr.tile->whichList = mOnClosedList; // Check the adjacent tiles for (int dy = -1; dy <= 1; dy++) { for (int dx = -1; dx <= 1; dx++) { // Calculate location of tile to check const int x = curr.x + dx; const int y = curr.y + dy; // Skip if if we're checking the same tile we're leaving from, // or if the new location falls outside of the map boundaries if ((dx == 0 && dy == 0) || !contains(x, y)) { continue; } MetaTile *newTile = getMetaTile(x, y); // Skip if the tile is on the closed list or collides unless // its the destination tile if (newTile->whichList == mOnClosedList || (tileCollides(x, y) && !(x == destX && y == destY))) { continue; } // When taking a diagonal step, verify that we can skip the // corner. We allow skipping past beings but not past non- // walkable tiles. if (dx != 0 && dy != 0) { MetaTile *t1 = getMetaTile(curr.x, curr.y + dy); MetaTile *t2 = getMetaTile(curr.x + dx, curr.y); if (!(t1->walkable && t2->walkable)) continue; } // Calculate G cost for this route, 10 for moving straight and // 14 for moving diagonal (sqrt(200) = 14.1421...) int Gcost = curr.tile->Gcost + ((dx == 0 || dy == 0) ? 10 : 14); // Skip if Gcost becomes too much // Warning: probably not entirely accurate if (Gcost > 200) { continue; } if (newTile->whichList != mOnOpenList) { // Found a new tile (not on open nor on closed list) // Update Hcost of the new tile using Manhatten distance newTile->Hcost = 10 * (abs(x - destX) + abs(y - destY)); // Set the current tile as the parent of the new tile newTile->parentX = curr.x; newTile->parentY = curr.y; // Update Gcost and Fcost of new tile newTile->Gcost = Gcost; newTile->Fcost = Gcost + newTile->Hcost; if (x != destX || y != destY) { // Add this tile to the open list newTile->whichList = mOnOpenList; openList.push(Location(x, y, newTile)); } else { // Target location was found foundPath = true; } } else if (Gcost < newTile->Gcost) { // Found a shorter route. // Update Gcost and Fcost of the new tile newTile->Gcost = Gcost; newTile->Fcost = Gcost + newTile->Hcost; // Set the current tile as the parent of the new tile newTile->parentX = curr.x; newTile->parentY = curr.y; // Add this tile to the open list (it's already // there, but this instance has a lower F score) openList.push(Location(x, y, newTile)); } } } } // Two new values to indicate whether a tile is on the open or closed list, // this way we don't have to clear all the values between each pathfinding. mOnClosedList += 2; mOnOpenList += 2; // If a path has been found, iterate backwards using the parent locations // to extract it. if (foundPath) { int pathX = destX; int pathY = destY; while (pathX != startX || pathY != startY) { // Add the new path node to the start of the path list path.push_front(Position(pathX, pathY)); // Find out the next parent MetaTile *tile = getMetaTile(pathX, pathY); pathX = tile->parentX; pathY = tile->parentY; } } return path; }