void NavGrid::UpdateNodeValues(Node* aParent, Node* aNodeToUpdate, int aGoalIndex)
{
aNodeToUpdate->myH = CalculateHeuristic(aNodeToUpdate->myGridIndex, aGoalIndex);
aNodeToUpdate->myG = aParent->myG + myMoveCost;
aNodeToUpdate->myF = aNodeToUpdate->myG + aNodeToUpdate->myH;
aNodeToUpdate->myParent = aParent;
}
void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& path)
{
UpdateGrid();
PROFILE("ComputePath");
PathfinderState state = { 0 };
// Convert the start/end coordinates to tile indexes
u16 i0, j0;
NearestTile(x0, z0, i0, j0);
NearestTile(goal.x, goal.z, state.iGoal, state.jGoal);
// If we're already at the goal tile, then move directly to the exact goal coordinates
if (AtGoal(i0, j0, goal))
{
Waypoint w = { goal.x, goal.z };
path.m_Waypoints.push_back(w);
return;
}
// If the target is a circle, we want to aim for the edge of it (so e.g. if we're inside
// a large circle then the heuristics will aim us directly outwards);
// otherwise just aim at the center point. (We'll never try moving outwards to a square shape.)
if (goal.type == Goal::CIRCLE)
state.rGoal = (goal.hw / (int)CELL_SIZE).ToInt_RoundToZero();
else
state.rGoal = 0;
state.passClass = passClass;
state.moveCosts = m_MoveCosts.at(costClass);
state.steps = 0;
state.tiles = new PathfindTileGrid(m_MapSize, m_MapSize);
state.terrain = m_Grid;
state.iBest = i0;
state.jBest = j0;
state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal, state.rGoal);
PriorityQueue::Item start = { std::make_pair(i0, j0), 0 };
state.open.push(start);
state.tiles->get(i0, j0).SetStatusOpen();
state.tiles->get(i0, j0).SetPred(i0, j0, i0, j0);
state.tiles->get(i0, j0).cost = 0;
// To prevent units getting very stuck, if they start on an impassable tile
// surrounded entirely by impassable tiles, we ignore the impassability
state.ignoreImpassable = !IS_PASSABLE(state.terrain->get(i0, j0), state.passClass);
while (1)
{
++state.steps;
// Hack to avoid spending ages computing giant paths, particularly when
// the destination is unreachable
if (state.steps > 40000)
break;
// If we ran out of tiles to examine, give up
if (state.open.empty())
break;
#if PATHFIND_STATS
state.sumOpenSize += state.open.size();
#endif
// Move best tile from open to closed
PriorityQueue::Item curr = state.open.pop();
u16 i = curr.id.first;
u16 j = curr.id.second;
state.tiles->get(i, j).SetStatusClosed();
// If we've reached the destination, stop
if (AtGoal(i, j, goal))
{
state.iBest = i;
state.jBest = j;
state.hBest = 0;
break;
}
// As soon as we find an escape route from the impassable terrain,
// take it and forbid any further use of impassable tiles
if (state.ignoreImpassable)
{
if (i > 0 && IS_PASSABLE(state.terrain->get(i-1, j), state.passClass))
state.ignoreImpassable = false;
else if (i < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i+1, j), state.passClass))
state.ignoreImpassable = false;
else if (j > 0 && IS_PASSABLE(state.terrain->get(i, j-1), state.passClass))
state.ignoreImpassable = false;
else if (j < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i, j+1), state.passClass))
state.ignoreImpassable = false;
}
u32 g = state.tiles->get(i, j).cost;
if (i > 0)
ProcessNeighbour(i, j, i-1, j, g, state);
if (i < m_MapSize-1)
ProcessNeighbour(i, j, i+1, j, g, state);
if (j > 0)
ProcessNeighbour(i, j, i, j-1, g, state);
if (j < m_MapSize-1)
ProcessNeighbour(i, j, i, j+1, g, state);
}
// Reconstruct the path (in reverse)
u16 ip = state.iBest, jp = state.jBest;
while (ip != i0 || jp != j0)
{
PathfindTile& n = state.tiles->get(ip, jp);
entity_pos_t x, z;
TileCenter(ip, jp, x, z);
Waypoint w = { x, z };
path.m_Waypoints.push_back(w);
// Follow the predecessor link
ip = n.GetPredI(ip);
jp = n.GetPredJ(jp);
}
// Save this grid for debug display
delete m_DebugGrid;
m_DebugGrid = state.tiles;
m_DebugSteps = state.steps;
#if PATHFIND_STATS
printf("PATHFINDER: steps=%d avgo=%d proc=%d impc=%d impo=%d addo=%d\n", state.steps, state.sumOpenSize/state.steps, state.numProcessed, state.numImproveClosed, state.numImproveOpen, state.numAddToOpen);
#endif
}
// Do the A* processing for a neighbour tile i,j.
static void ProcessNeighbour(u16 pi, u16 pj, u16 i, u16 j, u32 pg, PathfinderState& state)
{
#if PATHFIND_STATS
state.numProcessed++;
#endif
// Reject impassable tiles
TerrainTile tileTag = state.terrain->get(i, j);
if (!IS_PASSABLE(tileTag, state.passClass) && !state.ignoreImpassable)
return;
u32 dg = CalculateCostDelta(pi, pj, i, j, state.tiles, state.moveCosts.at(GET_COST_CLASS(tileTag)));
u32 g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor
PathfindTile& n = state.tiles->get(i, j);
// If this is a new tile, compute the heuristic distance
if (n.IsUnexplored())
{
n.h = CalculateHeuristic(i, j, state.iGoal, state.jGoal, state.rGoal);
// Remember the best tile we've seen so far, in case we never actually reach the target
if (n.h < state.hBest)
{
state.hBest = n.h;
state.iBest = i;
state.jBest = j;
}
}
else
{
// If we've already seen this tile, and the new path to this tile does not have a
// better cost, then stop now
if (g >= n.cost)
return;
// Otherwise, we have a better path.
// If we've already added this tile to the open list:
if (n.IsOpen())
{
// This is a better path, so replace the old one with the new cost/parent
n.cost = g;
n.SetPred(pi, pj, i, j);
n.SetStep(state.steps);
state.open.promote(std::make_pair(i, j), g + n.h);
#if PATHFIND_STATS
state.numImproveOpen++;
#endif
return;
}
// If we've already found the 'best' path to this tile:
if (n.IsClosed())
{
// This is a better path (possible when we use inadmissible heuristics), so reopen it
#if PATHFIND_STATS
state.numImproveClosed++;
#endif
// (fall through)
}
}
// Add it to the open list:
n.SetStatusOpen();
n.cost = g;
n.SetPred(pi, pj, i, j);
n.SetStep(state.steps);
PriorityQueue::Item t = { std::make_pair(i, j), g + n.h };
state.open.push(t);
#if PATHFIND_STATS
state.numAddToOpen++;
#endif
}
bool NavGrid::FindPath(const CU::Vector2i& aStartPoint, const CU::Vector2i& aEndPoint, CU::GrowingArray<CU::Vector2f>& aOutPath)
{
aOutPath.RemoveAll();
ResetGrid();
int startIndex = WorldIndexToGridIndex(aStartPoint);
int goalIndex = WorldIndexToGridIndex(aEndPoint);
if (!IsPositionOnGrid(aStartPoint) || !IsPositionOnGrid(aEndPoint))
return false;
Node& startNode = myGrid[startIndex];
startNode.myG = 0;
startNode.myH = CalculateHeuristic(startIndex, goalIndex);
startNode.myF = startNode.myG + startNode.myH;
startNode.myListState = IN_OPEN;
Node* currentClosestNode = &myGrid[startIndex];
CU::Heap<Node*, NavGrid_NodeComparer<Node*>> heap;
heap.Enqueue(&startNode);
while (!heap.IsEmpty())
{
Node* currNode = heap.Dequeue();
currNode->myListState = IN_CLOSED;
if (CalculateHeuristic(currNode->myGridIndex, goalIndex) < currentClosestNode->myH)
currentClosestNode = currNode;
if (currNode->myGridIndex == goalIndex)
{
while (currNode != nullptr)
{
CU::Vector2f worldPosition = ArrayIndexToWorldPosition(currNode->myGridIndex);
aOutPath.Add(worldPosition);
currNode = currNode->myParent;
}
return true;
}
int currIndex = currNode->myGridIndex;
if (currIndex > myGridSize)
{
ExamineNeighbour(currNode, currIndex - myGridSize, goalIndex, heap);
}
if (currIndex < myGridSize * (myGridSize - 1))
{
ExamineNeighbour(currNode, currIndex + myGridSize, goalIndex, heap);
}
if (currIndex % myGridSize != 0)
{
ExamineNeighbour(currNode, currIndex - 1, goalIndex, heap);
}
if ((currIndex + 1) % myGridSize != 0)
{
ExamineNeighbour(currNode, currIndex + 1, goalIndex, heap);
}
currNode->myListState = IN_CLOSED;
}
return FindPath(aStartPoint, ArrayIndexToGridIndex(currentClosestNode->myGridIndex), aOutPath);
}
