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