/** * Given a grid of passable/impassable navcells (based on some passability mask), * computes a new grid where a navcell is impassable (per that mask) if * it is <=clearance navcells away from an impassable navcell in the original grid. * The results are ORed onto the original grid. * * This is used for adding clearance onto terrain-based navcell passability. * * TODO PATHFINDER: might be nicer to get rounded corners by measuring clearances as * Euclidean distances; currently it effectively does dist=max(dx,dy) instead. * This would only really be a problem for big clearances. */ static void ExpandImpassableCells(Grid<u16>& grid, u16 clearance, pass_class_t mask) { PROFILE3("ExpandImpassableCells"); u16 w = grid.m_W; u16 h = grid.m_H; // First expand impassable cells horizontally into a temporary 1-bit grid Grid<u8> tempGrid(w, h); for (u16 j = 0; j < h; ++j) { // New cell (i,j) is blocked if (i',j) blocked for any i-clearance <= i' <= i+clearance // Count the number of blocked cells around i=0 u16 numBlocked = 0; for (u16 i = 0; i <= clearance && i < w; ++i) if (!IS_PASSABLE(grid.get(i, j), mask)) ++numBlocked; for (u16 i = 0; i < w; ++i) { // Store a flag if blocked by at least one nearby cell if (numBlocked) tempGrid.set(i, j, 1); // Slide the numBlocked window along: // remove the old i-clearance value, add the new (i+1)+clearance // (avoiding overflowing the grid) if (i >= clearance && !IS_PASSABLE(grid.get(i-clearance, j), mask)) --numBlocked; if (i+1+clearance < w && !IS_PASSABLE(grid.get(i+1+clearance, j), mask)) ++numBlocked; } } for (u16 i = 0; i < w; ++i) { // New cell (i,j) is blocked if (i,j') blocked for any j-clearance <= j' <= j+clearance // Count the number of blocked cells around j=0 u16 numBlocked = 0; for (u16 j = 0; j <= clearance && j < h; ++j) if (tempGrid.get(i, j)) ++numBlocked; for (u16 j = 0; j < h; ++j) { // Add the mask if blocked by at least one nearby cell if (numBlocked) grid.set(i, j, grid.get(i, j) | mask); // Slide the numBlocked window along: // remove the old j-clearance value, add the new (j+1)+clearance // (avoiding overflowing the grid) if (j >= clearance && tempGrid.get(i, j-clearance)) --numBlocked; if (j+1+clearance < h && tempGrid.get(i, j+1+clearance)) ++numBlocked; } } }
virtual void ProcessTile(ssize_t i, ssize_t j) { if (m_Pathfinder.m_Grid && !IS_PASSABLE(m_Pathfinder.m_Grid->get(i, j), m_Pathfinder.m_DebugPassClass)) RenderTile(CColor(1, 0, 0, 0.6f), false); if (m_Pathfinder.m_DebugGrid) { PathfindTile& n = m_Pathfinder.m_DebugGrid->get(i, j); float c = clamp(n.GetStep() / (float)m_Pathfinder.m_DebugSteps, 0.f, 1.f); if (n.IsOpen()) RenderTile(CColor(1, 1, c, 0.6f), false); else if (n.IsClosed()) RenderTile(CColor(0, 1, c, 0.6f), false); } }
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 UNUSED(onlyCenterPoint)) { // Check unit obstruction CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSystemEntity()); 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: ICmpObstructionManager::ObstructionSquare square; CmpPtr<ICmpObstruction> cmpObstruction(GetSimContext(), id); if (!cmpObstruction || !cmpObstruction->GetObstructionSquare(square)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_NO_OBSTRUCTION; entity_pos_t expand; const PathfinderPassability* passability = GetPassabilityFromMask(passClass); if (passability) expand = passability->m_Clearance; SimRasterize::Spans spans; SimRasterize::RasterizeRectWithClearance(spans, square, expand, Pathfinding::NAVCELL_SIZE); for (SimRasterize::Span& span : spans) { i16 i0 = span.i0; i16 i1 = span.i1; i16 j = span.j; // Fail if any span extends outside the grid if (i0 < 0 || i1 > m_TerrainOnlyGrid->m_W || j < 0 || j > m_TerrainOnlyGrid->m_H) return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; // Fail if any span includes an impassable tile for (i16 i = i0; i < i1; ++i) if (!IS_PASSABLE(m_TerrainOnlyGrid->get(i, j), passClass)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; } return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; }
ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool UNUSED(onlyCenterPoint)) { // Check unit obstruction CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSystemEntity()); 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 and static obstructions: u16 i, j; Pathfinding::NearestNavcell(x, z, i, j, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); if (!IS_PASSABLE(m_Grid->get(i, j), passClass)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; // (Static obstructions will be redundantly tested against in both the // obstruction-shape test and navcell-passability test, which is slightly // inefficient but shouldn't affect behaviour) return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; }
void HierarchicalPathfinder::Chunk::InitRegions(int ci, int cj, Grid<NavcellData>* grid, pass_class_t passClass) { ENSURE(ci < 256 && cj < 256); // avoid overflows m_ChunkI = ci; m_ChunkJ = cj; memset(m_Regions, 0, sizeof(m_Regions)); int i0 = ci * CHUNK_SIZE; int j0 = cj * CHUNK_SIZE; int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W); int j1 = std::min(j0 + CHUNK_SIZE, (int)grid->m_H); // Efficiently flood-fill the m_Regions grid int regionID = 0; std::vector<u16> connect; u16* pCurrentID = NULL; u16 LeftID = 0; u16 DownID = 0; bool Checked = false; // prevent some unneccessary RootID calls connect.reserve(32); // TODO: What's a sensible number? connect.push_back(0); // connect[0] = 0 // Start by filling the grid with 0 for blocked, // and regionID for unblocked for (int j = j0; j < j1; ++j) { for (int i = i0; i < i1; ++i) { pCurrentID = &m_Regions[j-j0][i-i0]; if (!IS_PASSABLE(grid->get(i, j), passClass)) { *pCurrentID = 0; continue; } if (j > j0) DownID = m_Regions[j-1-j0][i-i0]; if (i == i0) LeftID = 0; else LeftID = m_Regions[j-j0][i-1-i0]; if (LeftID > 0) { *pCurrentID = LeftID; if (*pCurrentID != DownID && DownID > 0 && !Checked) { u16 id0 = RootID(DownID, connect); u16 id1 = RootID(LeftID, connect); Checked = true; // this avoids repeatedly connecting the same IDs if (id0 < id1) connect[id1] = id0; else if (id0 > id1) connect[id0] = id1; } else if (DownID == 0) Checked = false; } else if (DownID > 0) { *pCurrentID = DownID; Checked = false; } else { // New ID *pCurrentID = ++regionID; connect.push_back(regionID); Checked = false; } } } // Directly point the root ID m_NumRegions = 0; for (u16 i = 1; i < regionID+1; ++i) { if (connect[i] == i) ++m_NumRegions; else connect[i] = RootID(i, connect); } // Replace IDs by the root ID for (int j = 0; j < CHUNK_SIZE; ++j) for (int i = 0; i < CHUNK_SIZE; ++i) m_Regions[j][i] = connect[m_Regions[j][i]]; }
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 }