bool CCmpPathfinder::CheckUnitPlacement(const IObstructionTestFilter& filter,
entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass)
{
// Check unit obstruction
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
if (cmpObstructionManager.null())
return false;
if (cmpObstructionManager->TestUnitShape(filter, x, z, r, NULL))
return false;
// Test against terrain:
UpdateGrid();
u16 i0, j0, i1, j1;
NearestTile(x - r, z - r, i0, j0);
NearestTile(x + r, z + r, i1, j1);
for (u16 j = j0; j <= j1; ++j)
{
for (u16 i = i0; i <= i1; ++i)
{
if (!IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass))
{
return false;
}
}
}
return true;
}
ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckBuildingPlacement(const IObstructionTestFilter& filter,
entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w,
entity_pos_t h, entity_id_t id, pass_class_t passClass, bool onlyCenterPoint)
{
// Check unit obstruction
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
if (!cmpObstructionManager)
return ICmpObstruction::FOUNDATION_CHECK_FAIL_ERROR;
if (cmpObstructionManager->TestStaticShape(filter, x, z, a, w, h, NULL))
return ICmpObstruction::FOUNDATION_CHECK_FAIL_OBSTRUCTS_FOUNDATION;
// Test against terrain:
UpdateGrid();
ICmpObstructionManager::ObstructionSquare square;
CmpPtr<ICmpObstruction> cmpObstruction(GetSimContext(), id);
if (!cmpObstruction || !cmpObstruction->GetObstructionSquare(square))
return ICmpObstruction::FOUNDATION_CHECK_FAIL_NO_OBSTRUCTION;
if (onlyCenterPoint)
{
u16 i, j;
NearestTile(x, z, i, j);
if (IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass))
return ICmpObstruction::FOUNDATION_CHECK_SUCCESS;
return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS;
}
// Expand bounds by 1/sqrt(2) tile (multiply by TERRAIN_TILE_SIZE since we want world coordinates)
entity_pos_t expand = entity_pos_t::FromInt(2).Sqrt().Multiply(entity_pos_t::FromInt(TERRAIN_TILE_SIZE / 2));
CFixedVector2D halfSize(square.hw + expand, square.hh + expand);
CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(square.u, square.v, halfSize);
u16 i0, j0, i1, j1;
NearestTile(square.x - halfBound.X, square.z - halfBound.Y, i0, j0);
NearestTile(square.x + halfBound.X, square.z + halfBound.Y, i1, j1);
for (u16 j = j0; j <= j1; ++j)
{
for (u16 i = i0; i <= i1; ++i)
{
entity_pos_t x, z;
TileCenter(i, j, x, z);
if (Geometry::PointIsInSquare(CFixedVector2D(x - square.x, z - square.z), square.u, square.v, halfSize)
&& !IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass))
{
return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS;
}
}
}
return ICmpObstruction::FOUNDATION_CHECK_SUCCESS;
}
fixed CCmpPathfinder::GetMovementSpeed(entity_pos_t x0, entity_pos_t z0, u8 costClass)
{
UpdateGrid();
u16 i, j;
NearestTile(x0, z0, i, j);
TerrainTile tileTag = m_Grid->get(i, j);
return m_MoveSpeeds.at(costClass).at(GET_COST_CLASS(tileTag));
}
bool CCmpPathfinder::CheckMovement(const IObstructionTestFilter& filter,
entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r,
pass_class_t passClass)
{
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
if (cmpObstructionManager.null())
return false;
if (cmpObstructionManager->TestLine(filter, x0, z0, x1, z1, r))
return false;
// Test against terrain:
// (TODO: this could probably be a tiny bit faster by not reusing all the vertex computation code)
UpdateGrid();
std::vector<Edge> edgesAA;
std::vector<Vertex> vertexes;
u16 i0, j0, i1, j1;
NearestTile(std::min(x0, x1) - r, std::min(z0, z1) - r, i0, j0);
NearestTile(std::max(x0, x1) + r, std::max(z0, z1) + r, i1, j1);
AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
CFixedVector2D a(x0, z0);
CFixedVector2D b(x1, z1);
std::vector<EdgeAA> edgesLeft;
std::vector<EdgeAA> edgesRight;
std::vector<EdgeAA> edgesBottom;
std::vector<EdgeAA> edgesTop;
SplitAAEdges(a, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
bool visible =
CheckVisibilityLeft(a, b, edgesLeft) &&
CheckVisibilityRight(a, b, edgesRight) &&
CheckVisibilityBottom(a, b, edgesBottom) &&
CheckVisibilityTop(a, b, edgesTop);
return visible;
}
ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckUnitPlacement(const IObstructionTestFilter& filter,
entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint)
{
// Check unit obstruction
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
if (!cmpObstructionManager)
return ICmpObstruction::FOUNDATION_CHECK_FAIL_ERROR;
if (cmpObstructionManager->TestUnitShape(filter, x, z, r, NULL))
return ICmpObstruction::FOUNDATION_CHECK_FAIL_OBSTRUCTS_FOUNDATION;
// Test against terrain:
UpdateGrid();
if (onlyCenterPoint)
{
u16 i, j;
NearestTile(x , z, i, j);
if (IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass))
return ICmpObstruction::FOUNDATION_CHECK_SUCCESS;
return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS;
}
u16 i0, j0, i1, j1;
NearestTile(x - r, z - r, i0, j0);
NearestTile(x + r, z + r, i1, j1);
for (u16 j = j0; j <= j1; ++j)
{
for (u16 i = i0; i <= i1; ++i)
{
if (!IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass))
{
return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS;
}
}
}
return ICmpObstruction::FOUNDATION_CHECK_SUCCESS;
}
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
entity_pos_t x0, entity_pos_t z0, entity_pos_t r,
entity_pos_t range, const Goal& goal, pass_class_t passClass, Path& path)
{
UpdateGrid(); // TODO: only need to bother updating if the terrain changed
PROFILE("ComputeShortPath");
// ScopeTimer UID__(L"ComputeShortPath");
m_DebugOverlayShortPathLines.clear();
if (m_DebugOverlay)
{
// Render the goal shape
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1);
switch (goal.type)
{
case CCmpPathfinder::Goal::POINT:
{
SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true);
break;
}
case CCmpPathfinder::Goal::CIRCLE:
{
SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true);
break;
}
case CCmpPathfinder::Goal::SQUARE:
{
float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat());
SimRender::ConstructSquareOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true);
break;
}
}
}
// List of collision edges - paths must never cross these.
// (Edges are one-sided so intersections are fine in one direction, but not the other direction.)
std::vector<Edge> edges;
std::vector<Edge> edgesAA; // axis-aligned squares
// Create impassable edges at the max-range boundary, so we can't escape the region
// where we're meant to be searching
fixed rangeXMin = x0 - range;
fixed rangeXMax = x0 + range;
fixed rangeZMin = z0 - range;
fixed rangeZMax = z0 + range;
{
// (The edges are the opposite direction to usual, so it's an inside-out square)
Edge e0 = { CFixedVector2D(rangeXMin, rangeZMin), CFixedVector2D(rangeXMin, rangeZMax) };
Edge e1 = { CFixedVector2D(rangeXMin, rangeZMax), CFixedVector2D(rangeXMax, rangeZMax) };
Edge e2 = { CFixedVector2D(rangeXMax, rangeZMax), CFixedVector2D(rangeXMax, rangeZMin) };
Edge e3 = { CFixedVector2D(rangeXMax, rangeZMin), CFixedVector2D(rangeXMin, rangeZMin) };
edges.push_back(e0);
edges.push_back(e1);
edges.push_back(e2);
edges.push_back(e3);
}
// List of obstruction vertexes (plus start/end points); we'll try to find paths through
// the graph defined by these vertexes
std::vector<Vertex> vertexes;
// Add the start point to the graph
CFixedVector2D posStart(x0, z0);
fixed hStart = (posStart - NearestPointOnGoal(posStart, goal)).Length();
Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL };
vertexes.push_back(start);
const size_t START_VERTEX_ID = 0;
// Add the goal vertex to the graph.
// Since the goal isn't always a point, this a special magic virtual vertex which moves around - whenever
// we look at it from another vertex, it is moved to be the closest point on the goal shape to that vertex.
Vertex end = { CFixedVector2D(goal.x, goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL };
vertexes.push_back(end);
const size_t GOAL_VERTEX_ID = 1;
// Add terrain obstructions
{
u16 i0, j0, i1, j1;
NearestTile(rangeXMin, rangeZMin, i0, j0);
NearestTile(rangeXMax, rangeZMax, i1, j1);
AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
}
// Find all the obstruction squares that might affect us
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
std::vector<ICmpObstructionManager::ObstructionSquare> squares;
cmpObstructionManager->GetObstructionsInRange(filter, rangeXMin - r, rangeZMin - r, rangeXMax + r, rangeZMax + r, squares);
// Resize arrays to reduce reallocations
vertexes.reserve(vertexes.size() + squares.size()*4);
edgesAA.reserve(edgesAA.size() + squares.size()); // (assume most squares are AA)
// Convert each obstruction square into collision edges and search graph vertexes
for (size_t i = 0; i < squares.size(); ++i)
{
CFixedVector2D center(squares[i].x, squares[i].z);
CFixedVector2D u = squares[i].u;
CFixedVector2D v = squares[i].v;
// Expand the vertexes by the moving unit's collision radius, to find the
// closest we can get to it
CFixedVector2D hd0(squares[i].hw + r + EDGE_EXPAND_DELTA, squares[i].hh + r + EDGE_EXPAND_DELTA);
CFixedVector2D hd1(squares[i].hw + r + EDGE_EXPAND_DELTA, -(squares[i].hh + r + EDGE_EXPAND_DELTA));
// Check whether this is an axis-aligned square
bool aa = (u.X == fixed::FromInt(1) && u.Y == fixed::Zero() && v.X == fixed::Zero() && v.Y == fixed::FromInt(1));
Vertex vert;
vert.status = Vertex::UNEXPLORED;
vert.quadInward = QUADRANT_NONE;
vert.quadOutward = QUADRANT_ALL;
vert.p.X = center.X - hd0.Dot(u); vert.p.Y = center.Y + hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_BR; vertexes.push_back(vert);
vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_TR; vertexes.push_back(vert);
vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_TL; vertexes.push_back(vert);
vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; vertexes.push_back(vert);
// Add the edges:
CFixedVector2D h0(squares[i].hw + r, squares[i].hh + r);
CFixedVector2D h1(squares[i].hw + r, -(squares[i].hh + r));
CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v));
CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v));
CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v));
CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v));
if (aa)
{
Edge e = { ev1, ev3 };
edgesAA.push_back(e);
}
else
{
Edge e0 = { ev0, ev1 };
Edge e1 = { ev1, ev2 };
Edge e2 = { ev2, ev3 };
Edge e3 = { ev3, ev0 };
edges.push_back(e0);
edges.push_back(e1);
edges.push_back(e2);
edges.push_back(e3);
}
// TODO: should clip out vertexes and edges that are outside the range,
// to reduce the search space
}
ENSURE(vertexes.size() < 65536); // we store array indexes as u16
if (m_DebugOverlay)
{
// Render the obstruction edges
for (size_t i = 0; i < edges.size(); ++i)
{
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
std::vector<float> xz;
xz.push_back(edges[i].p0.X.ToFloat());
xz.push_back(edges[i].p0.Y.ToFloat());
xz.push_back(edges[i].p1.X.ToFloat());
xz.push_back(edges[i].p1.Y.ToFloat());
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
}
for (size_t i = 0; i < edgesAA.size(); ++i)
{
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
std::vector<float> xz;
xz.push_back(edgesAA[i].p0.X.ToFloat());
xz.push_back(edgesAA[i].p0.Y.ToFloat());
xz.push_back(edgesAA[i].p0.X.ToFloat());
xz.push_back(edgesAA[i].p1.Y.ToFloat());
xz.push_back(edgesAA[i].p1.X.ToFloat());
xz.push_back(edgesAA[i].p1.Y.ToFloat());
xz.push_back(edgesAA[i].p1.X.ToFloat());
xz.push_back(edgesAA[i].p0.Y.ToFloat());
xz.push_back(edgesAA[i].p0.X.ToFloat());
xz.push_back(edgesAA[i].p0.Y.ToFloat());
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
}
}
// Do an A* search over the vertex/visibility graph:
// Since we are just measuring Euclidean distance the heuristic is admissible,
// so we never have to re-examine a node once it's been moved to the closed set.
// To save time in common cases, we don't precompute a graph of valid edges between vertexes;
// we do it lazily instead. When the search algorithm reaches a vertex, we examine every other
// vertex and see if we can reach it without hitting any collision edges, and ignore the ones
// we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked
// as closed), we won't be doing any redundant visibility computations.
PROFILE_START("A*");
PriorityQueue open;
PriorityQueue::Item qiStart = { START_VERTEX_ID, start.h };
open.push(qiStart);
u16 idBest = START_VERTEX_ID;
fixed hBest = start.h;
while (!open.empty())
{
// Move best tile from open to closed
PriorityQueue::Item curr = open.pop();
vertexes[curr.id].status = Vertex::CLOSED;
// If we've reached the destination, stop
if (curr.id == GOAL_VERTEX_ID)
{
idBest = curr.id;
break;
}
// Sort the edges so ones nearer this vertex are checked first by CheckVisibility,
// since they're more likely to block the rays
std::sort(edgesAA.begin(), edgesAA.end(), EdgeSort(vertexes[curr.id].p));
std::vector<EdgeAA> edgesLeft;
std::vector<EdgeAA> edgesRight;
std::vector<EdgeAA> edgesBottom;
std::vector<EdgeAA> edgesTop;
SplitAAEdges(vertexes[curr.id].p, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
// Check the lines to every other vertex
for (size_t n = 0; n < vertexes.size(); ++n)
{
if (vertexes[n].status == Vertex::CLOSED)
continue;
// If this is the magical goal vertex, move it to near the current vertex
CFixedVector2D npos;
if (n == GOAL_VERTEX_ID)
{
npos = NearestPointOnGoal(vertexes[curr.id].p, goal);
// To prevent integer overflows later on, we need to ensure all vertexes are
// 'close' to the source. The goal might be far away (not a good idea but
// sometimes it happens), so clamp it to the current search range
npos.X = clamp(npos.X, rangeXMin, rangeXMax);
npos.Y = clamp(npos.Y, rangeZMin, rangeZMax);
}
else
{
npos = vertexes[n].p;
}
// Work out which quadrant(s) we're approaching the new vertex from
u8 quad = 0;
if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL;
if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR;
if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL;
if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BR;
// Check that the new vertex is in the right quadrant for the old vertex
if (!(vertexes[curr.id].quadOutward & quad))
{
// Hack: Always head towards the goal if possible, to avoid missing it if it's
// inside another unit
if (n != GOAL_VERTEX_ID)
{
continue;
}
}
bool visible =
CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) &&
CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) &&
CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) &&
CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) &&
CheckVisibility(vertexes[curr.id].p, npos, edges);
/*
// Render the edges that we examine
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5);
std::vector<float> xz;
xz.push_back(vertexes[curr.id].p.X.ToFloat());
xz.push_back(vertexes[curr.id].p.Y.ToFloat());
xz.push_back(npos.X.ToFloat());
xz.push_back(npos.Y.ToFloat());
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false);
//*/
if (visible)
{
fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length();
// If this is a new tile, compute the heuristic distance
if (vertexes[n].status == Vertex::UNEXPLORED)
{
// Add it to the open list:
vertexes[n].status = Vertex::OPEN;
vertexes[n].g = g;
vertexes[n].h = DistanceToGoal(npos, goal);
vertexes[n].pred = curr.id;
// If this is an axis-aligned shape, the path must continue in the same quadrant
// direction (but not go into the inside of the shape).
// Hack: If we started *inside* a shape then perhaps headed to its corner (e.g. the unit
// was very near another unit), don't restrict further pathing.
if (vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8)))
vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
if (n == GOAL_VERTEX_ID)
vertexes[n].p = npos; // remember the new best goal position
PriorityQueue::Item t = { (u16)n, g + vertexes[n].h };
open.push(t);
// Remember the heuristically best vertex we've seen so far, in case we never actually reach the target
if (vertexes[n].h < hBest)
{
idBest = (u16)n;
hBest = vertexes[n].h;
}
}
else // must be OPEN
{
// 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 >= vertexes[n].g)
continue;
// Otherwise, we have a better path, so replace the old one with the new cost/parent
vertexes[n].g = g;
vertexes[n].pred = curr.id;
// If this is an axis-aligned shape, the path must continue in the same quadrant
// direction (but not go into the inside of the shape).
if (vertexes[n].quadInward)
vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
if (n == GOAL_VERTEX_ID)
vertexes[n].p = npos; // remember the new best goal position
open.promote((u16)n, g + vertexes[n].h);
}
}
}
}
// Reconstruct the path (in reverse)
for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred)
{
Waypoint w = { vertexes[id].p.X, vertexes[id].p.Y };
path.m_Waypoints.push_back(w);
}
PROFILE_END("A*");
}
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
}
