Index: ps/trunk/source/simulation2/components/CCmpPathfinder.cpp =================================================================== --- ps/trunk/source/simulation2/components/CCmpPathfinder.cpp (revision 22304) +++ ps/trunk/source/simulation2/components/CCmpPathfinder.cpp (revision 22305) @@ -1,930 +1,930 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ /** * @file * Common code and setup code for CCmpPathfinder. */ #include "precompiled.h" #include "CCmpPathfinder_Common.h" #include "ps/CLogger.h" #include "ps/CStr.h" #include "ps/Profile.h" #include "ps/XML/Xeromyces.h" #include "renderer/Scene.h" #include "simulation2/MessageTypes.h" #include "simulation2/components/ICmpObstruction.h" #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/components/ICmpTerrain.h" #include "simulation2/components/ICmpWaterManager.h" #include "simulation2/helpers/HierarchicalPathfinder.h" #include "simulation2/helpers/LongPathfinder.h" #include "simulation2/helpers/Rasterize.h" #include "simulation2/helpers/VertexPathfinder.h" #include "simulation2/serialization/SerializeTemplates.h" REGISTER_COMPONENT_TYPE(Pathfinder) void CCmpPathfinder::Init(const CParamNode& UNUSED(paramNode)) { m_MapSize = 0; m_Grid = NULL; m_TerrainOnlyGrid = NULL; FlushAIPathfinderDirtinessInformation(); m_NextAsyncTicket = 1; m_AtlasOverlay = NULL; m_SameTurnMovesCount = 0; m_VertexPathfinder = std::unique_ptr(new VertexPathfinder(m_MapSize, m_TerrainOnlyGrid)); m_LongPathfinder = std::unique_ptr(new LongPathfinder()); m_PathfinderHier = std::unique_ptr(new HierarchicalPathfinder()); // Register Relax NG validator CXeromyces::AddValidator(g_VFS, "pathfinder", "simulation/data/pathfinder.rng"); // Since this is used as a system component (not loaded from an entity template), // we can't use the real paramNode (it won't get handled properly when deserializing), // so load the data from a special XML file. CParamNode externalParamNode; CParamNode::LoadXML(externalParamNode, L"simulation/data/pathfinder.xml", "pathfinder"); // Previously all move commands during a turn were // queued up and processed asynchronously at the start // of the next turn. Now we are processing queued up // events several times duing the turn. This improves // responsiveness and units move more smoothly especially. // when in formation. There is still a call at the // beginning of a turn to process all outstanding moves - // this will handle any moves above the MaxSameTurnMoves // threshold. // // TODO - The moves processed at the beginning of the // turn do not count against the maximum moves per turn // currently. The thinking is that this will eventually // happen in another thread. Either way this probably // will require some adjustment and rethinking. const CParamNode pathingSettings = externalParamNode.GetChild("Pathfinder"); m_MaxSameTurnMoves = (u16)pathingSettings.GetChild("MaxSameTurnMoves").ToInt(); const CParamNode::ChildrenMap& passClasses = externalParamNode.GetChild("Pathfinder").GetChild("PassabilityClasses").GetChildren(); for (CParamNode::ChildrenMap::const_iterator it = passClasses.begin(); it != passClasses.end(); ++it) { std::string name = it->first; ENSURE((int)m_PassClasses.size() <= PASS_CLASS_BITS); pass_class_t mask = PASS_CLASS_MASK_FROM_INDEX(m_PassClasses.size()); m_PassClasses.push_back(PathfinderPassability(mask, it->second)); m_PassClassMasks[name] = mask; } } CCmpPathfinder::~CCmpPathfinder() {}; void CCmpPathfinder::Deinit() { SetDebugOverlay(false); // cleans up memory SAFE_DELETE(m_AtlasOverlay); SAFE_DELETE(m_Grid); SAFE_DELETE(m_TerrainOnlyGrid); } struct SerializeLongRequest { template - void operator()(S& serialize, const char* UNUSED(name), AsyncLongPathRequest& value) + void operator()(S& serialize, const char* UNUSED(name), LongPathRequest& value) { serialize.NumberU32_Unbounded("ticket", value.ticket); serialize.NumberFixed_Unbounded("x0", value.x0); serialize.NumberFixed_Unbounded("z0", value.z0); SerializeGoal()(serialize, "goal", value.goal); serialize.NumberU16_Unbounded("pass class", value.passClass); serialize.NumberU32_Unbounded("notify", value.notify); } }; struct SerializeShortRequest { template - void operator()(S& serialize, const char* UNUSED(name), AsyncShortPathRequest& value) + void operator()(S& serialize, const char* UNUSED(name), ShortPathRequest& value) { serialize.NumberU32_Unbounded("ticket", value.ticket); serialize.NumberFixed_Unbounded("x0", value.x0); serialize.NumberFixed_Unbounded("z0", value.z0); serialize.NumberFixed_Unbounded("clearance", value.clearance); serialize.NumberFixed_Unbounded("range", value.range); SerializeGoal()(serialize, "goal", value.goal); serialize.NumberU16_Unbounded("pass class", value.passClass); serialize.Bool("avoid moving units", value.avoidMovingUnits); serialize.NumberU32_Unbounded("group", value.group); serialize.NumberU32_Unbounded("notify", value.notify); } }; template void CCmpPathfinder::SerializeCommon(S& serialize) { - SerializeVector()(serialize, "long requests", m_AsyncLongPathRequests); - SerializeVector()(serialize, "short requests", m_AsyncShortPathRequests); + SerializeVector()(serialize, "long requests", m_LongPathRequests); + SerializeVector()(serialize, "short requests", m_ShortPathRequests); serialize.NumberU32_Unbounded("next ticket", m_NextAsyncTicket); serialize.NumberU16_Unbounded("same turn moves count", m_SameTurnMovesCount); serialize.NumberU16_Unbounded("map size", m_MapSize); } void CCmpPathfinder::Serialize(ISerializer& serialize) { SerializeCommon(serialize); } void CCmpPathfinder::Deserialize(const CParamNode& paramNode, IDeserializer& deserialize) { Init(paramNode); SerializeCommon(deserialize); } void CCmpPathfinder::HandleMessage(const CMessage& msg, bool UNUSED(global)) { switch (msg.GetType()) { case MT_RenderSubmit: { const CMessageRenderSubmit& msgData = static_cast (msg); RenderSubmit(msgData.collector); break; } case MT_TerrainChanged: m_TerrainDirty = true; MinimalTerrainUpdate(); break; case MT_WaterChanged: case MT_ObstructionMapShapeChanged: m_TerrainDirty = true; UpdateGrid(); break; case MT_TurnStart: m_SameTurnMovesCount = 0; break; } } void CCmpPathfinder::RenderSubmit(SceneCollector& collector) { m_VertexPathfinder->RenderSubmit(collector); m_PathfinderHier->RenderSubmit(collector); } void CCmpPathfinder::SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) { m_LongPathfinder->SetDebugPath(*m_PathfinderHier, x0, z0, goal, passClass); } void CCmpPathfinder::SetDebugOverlay(bool enabled) { m_VertexPathfinder->SetDebugOverlay(enabled); m_LongPathfinder->SetDebugOverlay(enabled); } void CCmpPathfinder::SetHierDebugOverlay(bool enabled) { m_PathfinderHier->SetDebugOverlay(enabled, &GetSimContext()); } void CCmpPathfinder::GetDebugData(u32& steps, double& time, Grid& grid) const { m_LongPathfinder->GetDebugData(steps, time, grid); } void CCmpPathfinder::SetAtlasOverlay(bool enable, pass_class_t passClass) { if (enable) { if (!m_AtlasOverlay) m_AtlasOverlay = new AtlasOverlay(this, passClass); m_AtlasOverlay->m_PassClass = passClass; } else SAFE_DELETE(m_AtlasOverlay); } pass_class_t CCmpPathfinder::GetPassabilityClass(const std::string& name) const { std::map::const_iterator it = m_PassClassMasks.find(name); if (it == m_PassClassMasks.end()) { LOGERROR("Invalid passability class name '%s'", name.c_str()); return 0; } return it->second; } void CCmpPathfinder::GetPassabilityClasses(std::map& passClasses) const { passClasses = m_PassClassMasks; } void CCmpPathfinder::GetPassabilityClasses(std::map& nonPathfindingPassClasses, std::map& pathfindingPassClasses) const { for (const std::pair& pair : m_PassClassMasks) { if ((GetPassabilityFromMask(pair.second)->m_Obstructions == PathfinderPassability::PATHFINDING)) pathfindingPassClasses[pair.first] = pair.second; else nonPathfindingPassClasses[pair.first] = pair.second; } } const PathfinderPassability* CCmpPathfinder::GetPassabilityFromMask(pass_class_t passClass) const { for (const PathfinderPassability& passability : m_PassClasses) { if (passability.m_Mask == passClass) return &passability; } return NULL; } const Grid& CCmpPathfinder::GetPassabilityGrid() { if (!m_Grid) UpdateGrid(); return *m_Grid; } /** * 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& 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 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; } } } Grid CCmpPathfinder::ComputeShoreGrid(bool expandOnWater) { PROFILE3("ComputeShoreGrid"); CmpPtr cmpWaterManager(GetSystemEntity()); // TODO: these bits should come from ICmpTerrain CTerrain& terrain = GetSimContext().GetTerrain(); // avoid integer overflow in intermediate calculation const u16 shoreMax = 32767; // First pass - find underwater tiles Grid waterGrid(m_MapSize, m_MapSize); for (u16 j = 0; j < m_MapSize; ++j) { for (u16 i = 0; i < m_MapSize; ++i) { fixed x, z; Pathfinding::TileCenter(i, j, x, z); bool underWater = cmpWaterManager && (cmpWaterManager->GetWaterLevel(x, z) > terrain.GetExactGroundLevelFixed(x, z)); waterGrid.set(i, j, underWater ? 1 : 0); } } // Second pass - find shore tiles Grid shoreGrid(m_MapSize, m_MapSize); for (u16 j = 0; j < m_MapSize; ++j) { for (u16 i = 0; i < m_MapSize; ++i) { // Find a land tile if (!waterGrid.get(i, j)) { // If it's bordered by water, it's a shore tile if ((i > 0 && waterGrid.get(i-1, j)) || (i > 0 && j < m_MapSize-1 && waterGrid.get(i-1, j+1)) || (i > 0 && j > 0 && waterGrid.get(i-1, j-1)) || (i < m_MapSize-1 && waterGrid.get(i+1, j)) || (i < m_MapSize-1 && j < m_MapSize-1 && waterGrid.get(i+1, j+1)) || (i < m_MapSize-1 && j > 0 && waterGrid.get(i+1, j-1)) || (j > 0 && waterGrid.get(i, j-1)) || (j < m_MapSize-1 && waterGrid.get(i, j+1)) ) shoreGrid.set(i, j, 0); else shoreGrid.set(i, j, shoreMax); } // If we want to expand on water, we want water tiles not to be shore tiles else if (expandOnWater) shoreGrid.set(i, j, shoreMax); } } // Expand influences on land to find shore distance for (u16 y = 0; y < m_MapSize; ++y) { u16 min = shoreMax; for (u16 x = 0; x < m_MapSize; ++x) { if (!waterGrid.get(x, y) || expandOnWater) { u16 g = shoreGrid.get(x, y); if (g > min) shoreGrid.set(x, y, min); else if (g < min) min = g; ++min; } } for (u16 x = m_MapSize; x > 0; --x) { if (!waterGrid.get(x-1, y) || expandOnWater) { u16 g = shoreGrid.get(x-1, y); if (g > min) shoreGrid.set(x-1, y, min); else if (g < min) min = g; ++min; } } } for (u16 x = 0; x < m_MapSize; ++x) { u16 min = shoreMax; for (u16 y = 0; y < m_MapSize; ++y) { if (!waterGrid.get(x, y) || expandOnWater) { u16 g = shoreGrid.get(x, y); if (g > min) shoreGrid.set(x, y, min); else if (g < min) min = g; ++min; } } for (u16 y = m_MapSize; y > 0; --y) { if (!waterGrid.get(x, y-1) || expandOnWater) { u16 g = shoreGrid.get(x, y-1); if (g > min) shoreGrid.set(x, y-1, min); else if (g < min) min = g; ++min; } } } return shoreGrid; } void CCmpPathfinder::UpdateGrid() { PROFILE3("UpdateGrid"); CmpPtr cmpTerrain(GetSimContext(), SYSTEM_ENTITY); if (!cmpTerrain) return; // error u16 terrainSize = cmpTerrain->GetTilesPerSide(); if (terrainSize == 0) return; // If the terrain was resized then delete the old grid data if (m_Grid && m_MapSize != terrainSize) { SAFE_DELETE(m_Grid); SAFE_DELETE(m_TerrainOnlyGrid); } // Initialise the terrain data when first needed if (!m_Grid) { m_MapSize = terrainSize; m_Grid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); SAFE_DELETE(m_TerrainOnlyGrid); m_TerrainOnlyGrid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); m_DirtinessInformation = { true, true, Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE) }; m_AIPathfinderDirtinessInformation = m_DirtinessInformation; m_TerrainDirty = true; } // The grid should be properly initialized and clean. Checking the latter is expensive so do it only for debugging. #ifdef NDEBUG ENSURE(m_DirtinessInformation.dirtinessGrid.compare_sizes(m_Grid)); #else ENSURE(m_DirtinessInformation.dirtinessGrid == Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE)); #endif CmpPtr cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY); cmpObstructionManager->UpdateInformations(m_DirtinessInformation); if (!m_DirtinessInformation.dirty && !m_TerrainDirty) return; // If the terrain has changed, recompute m_Grid // Else, use data from m_TerrainOnlyGrid and add obstructions if (m_TerrainDirty) { TerrainUpdateHelper(); *m_Grid = *m_TerrainOnlyGrid; m_TerrainDirty = false; m_DirtinessInformation.globallyDirty = true; } else if (m_DirtinessInformation.globallyDirty) { ENSURE(m_Grid->compare_sizes(m_TerrainOnlyGrid)); memcpy(m_Grid->m_Data, m_TerrainOnlyGrid->m_Data, (m_Grid->m_W)*(m_Grid->m_H)*sizeof(NavcellData)); } else { ENSURE(m_Grid->compare_sizes(m_TerrainOnlyGrid)); for (u16 j = 0; j < m_DirtinessInformation.dirtinessGrid.m_H; ++j) for (u16 i = 0; i < m_DirtinessInformation.dirtinessGrid.m_W; ++i) if (m_DirtinessInformation.dirtinessGrid.get(i, j) == 1) m_Grid->set(i, j, m_TerrainOnlyGrid->get(i, j)); } // Add obstructions onto the grid cmpObstructionManager->Rasterize(*m_Grid, m_PassClasses, m_DirtinessInformation.globallyDirty); // Update the long-range and hierarchical pathfinders. if (m_DirtinessInformation.globallyDirty) { std::map nonPathfindingPassClasses, pathfindingPassClasses; GetPassabilityClasses(nonPathfindingPassClasses, pathfindingPassClasses); m_LongPathfinder->Reload(m_Grid); m_PathfinderHier->Recompute(m_Grid, nonPathfindingPassClasses, pathfindingPassClasses); } else { m_LongPathfinder->Update(m_Grid); m_PathfinderHier->Update(m_Grid, m_DirtinessInformation.dirtinessGrid); } // Remember the necessary updates that the AI pathfinder will have to perform as well m_AIPathfinderDirtinessInformation.MergeAndClear(m_DirtinessInformation); } void CCmpPathfinder::MinimalTerrainUpdate() { TerrainUpdateHelper(false); } void CCmpPathfinder::TerrainUpdateHelper(bool expandPassability/* = true */) { PROFILE3("TerrainUpdateHelper"); CmpPtr cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY); CmpPtr cmpWaterManager(GetSimContext(), SYSTEM_ENTITY); CmpPtr cmpTerrain(GetSimContext(), SYSTEM_ENTITY); CTerrain& terrain = GetSimContext().GetTerrain(); if (!cmpTerrain || !cmpObstructionManager) return; u16 terrainSize = cmpTerrain->GetTilesPerSide(); if (terrainSize == 0) return; if (!m_TerrainOnlyGrid || m_MapSize != terrainSize) { m_MapSize = terrainSize; SAFE_DELETE(m_TerrainOnlyGrid); m_TerrainOnlyGrid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); // If this update comes from a map resizing, we must reinitialize the other grids as well if (!m_TerrainOnlyGrid->compare_sizes(m_Grid)) { SAFE_DELETE(m_Grid); m_Grid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); m_DirtinessInformation = { true, true, Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE) }; m_AIPathfinderDirtinessInformation = m_DirtinessInformation; } } Grid shoreGrid = ComputeShoreGrid(); // Compute initial terrain-dependent passability for (int j = 0; j < m_MapSize * Pathfinding::NAVCELLS_PER_TILE; ++j) { for (int i = 0; i < m_MapSize * Pathfinding::NAVCELLS_PER_TILE; ++i) { // World-space coordinates for this navcell fixed x, z; Pathfinding::NavcellCenter(i, j, x, z); // Terrain-tile coordinates for this navcell int itile = i / Pathfinding::NAVCELLS_PER_TILE; int jtile = j / Pathfinding::NAVCELLS_PER_TILE; // Gather all the data potentially needed to determine passability: fixed height = terrain.GetExactGroundLevelFixed(x, z); fixed water; if (cmpWaterManager) water = cmpWaterManager->GetWaterLevel(x, z); fixed depth = water - height; // Exact slopes give kind of weird output, so just use rough tile-based slopes fixed slope = terrain.GetSlopeFixed(itile, jtile); // Get world-space coordinates from shoreGrid (which uses terrain tiles) fixed shoredist = fixed::FromInt(shoreGrid.get(itile, jtile)).MultiplyClamp(TERRAIN_TILE_SIZE); // Compute the passability for every class for this cell NavcellData t = 0; for (PathfinderPassability& passability : m_PassClasses) if (!passability.IsPassable(depth, slope, shoredist)) t |= passability.m_Mask; m_TerrainOnlyGrid->set(i, j, t); } } // Compute off-world passability // WARNING: CCmpRangeManager::LosIsOffWorld needs to be kept in sync with this const int edgeSize = 3 * Pathfinding::NAVCELLS_PER_TILE; // number of tiles around the edge that will be off-world NavcellData edgeMask = 0; for (PathfinderPassability& passability : m_PassClasses) edgeMask |= passability.m_Mask; int w = m_TerrainOnlyGrid->m_W; int h = m_TerrainOnlyGrid->m_H; if (cmpObstructionManager->GetPassabilityCircular()) { for (int j = 0; j < h; ++j) { for (int i = 0; i < w; ++i) { // Based on CCmpRangeManager::LosIsOffWorld // but tweaked since it's tile-based instead. // (We double all the values so we can handle half-tile coordinates.) // This needs to be slightly tighter than the LOS circle, // else units might get themselves lost in the SoD around the edge. int dist2 = (i*2 + 1 - w)*(i*2 + 1 - w) + (j*2 + 1 - h)*(j*2 + 1 - h); if (dist2 >= (w - 2*edgeSize) * (h - 2*edgeSize)) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); } } } else { for (u16 j = 0; j < h; ++j) for (u16 i = 0; i < edgeSize; ++i) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); for (u16 j = 0; j < h; ++j) for (u16 i = w-edgeSize+1; i < w; ++i) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); for (u16 j = 0; j < edgeSize; ++j) for (u16 i = edgeSize; i < w-edgeSize+1; ++i) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); for (u16 j = h-edgeSize+1; j < h; ++j) for (u16 i = edgeSize; i < w-edgeSize+1; ++i) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); } if (!expandPassability) return; // Expand the impassability grid, for any class with non-zero clearance, // so that we can stop units getting too close to impassable navcells. // Note: It's not possible to perform this expansion once for all passabilities // with the same clearance, because the impassable cells are not necessarily the // same for all these passabilities. for (PathfinderPassability& passability : m_PassClasses) { if (passability.m_Clearance == fixed::Zero()) continue; int clearance = (passability.m_Clearance / Pathfinding::NAVCELL_SIZE).ToInt_RoundToInfinity(); ExpandImpassableCells(*m_TerrainOnlyGrid, clearance, passability.m_Mask); } } ////////////////////////////////////////////////////////// void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) const { m_LongPathfinder->ComputePath(*m_PathfinderHier, x0, z0, goal, passClass, ret); } u32 CCmpPathfinder::ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) { - AsyncLongPathRequest req = { m_NextAsyncTicket++, x0, z0, goal, passClass, notify }; - m_AsyncLongPathRequests.push_back(req); + LongPathRequest req = { m_NextAsyncTicket++, x0, z0, goal, passClass, notify }; + m_LongPathRequests.push_back(req); return req.ticket; } u32 CCmpPathfinder::ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t group, entity_id_t notify) { - AsyncShortPathRequest req = { m_NextAsyncTicket++, x0, z0, clearance, range, goal, passClass, avoidMovingUnits, group, notify }; - m_AsyncShortPathRequests.push_back(req); + ShortPathRequest req = { m_NextAsyncTicket++, x0, z0, clearance, range, goal, passClass, avoidMovingUnits, group, notify }; + m_ShortPathRequests.push_back(req); return req.ticket; } -WaypointPath CCmpPathfinder::ComputeShortPath(const AsyncShortPathRequest& request) const +WaypointPath CCmpPathfinder::ComputeShortPath(const ShortPathRequest& request) const { return m_VertexPathfinder->ComputeShortPath(request, CmpPtr(GetSystemEntity())); } // Async processing: void CCmpPathfinder::FinishAsyncRequests() { PROFILE2("Finish Async Requests"); // Save the request queue in case it gets modified while iterating - std::vector longRequests; - m_AsyncLongPathRequests.swap(longRequests); + std::vector longRequests; + m_LongPathRequests.swap(longRequests); - std::vector shortRequests; - m_AsyncShortPathRequests.swap(shortRequests); + std::vector shortRequests; + m_ShortPathRequests.swap(shortRequests); // TODO: we should only compute one path per entity per turn // TODO: this computation should be done incrementally, spread // across multiple frames (or even multiple turns) ProcessLongRequests(longRequests); ProcessShortRequests(shortRequests); } -void CCmpPathfinder::ProcessLongRequests(const std::vector& longRequests) +void CCmpPathfinder::ProcessLongRequests(const std::vector& longRequests) { PROFILE2("Process Long Requests"); for (size_t i = 0; i < longRequests.size(); ++i) { - const AsyncLongPathRequest& req = longRequests[i]; + const LongPathRequest& req = longRequests[i]; WaypointPath path; ComputePath(req.x0, req.z0, req.goal, req.passClass, path); CMessagePathResult msg(req.ticket, path); GetSimContext().GetComponentManager().PostMessage(req.notify, msg); } } -void CCmpPathfinder::ProcessShortRequests(const std::vector& shortRequests) +void CCmpPathfinder::ProcessShortRequests(const std::vector& shortRequests) { PROFILE2("Process Short Requests"); for (size_t i = 0; i < shortRequests.size(); ++i) { - const AsyncShortPathRequest& req = shortRequests[i]; + const ShortPathRequest& req = shortRequests[i]; WaypointPath path = m_VertexPathfinder->ComputeShortPath(req, CmpPtr(GetSystemEntity())); CMessagePathResult msg(req.ticket, path); GetSimContext().GetComponentManager().PostMessage(req.notify, msg); } } void CCmpPathfinder::ProcessSameTurnMoves() { - if (!m_AsyncLongPathRequests.empty()) + if (!m_LongPathRequests.empty()) { // Figure out how many moves we can do this time i32 moveCount = m_MaxSameTurnMoves - m_SameTurnMovesCount; if (moveCount <= 0) return; // Copy the long request elements we are going to process into a new array - std::vector longRequests; - if ((i32)m_AsyncLongPathRequests.size() <= moveCount) + std::vector longRequests; + if ((i32)m_LongPathRequests.size() <= moveCount) { - m_AsyncLongPathRequests.swap(longRequests); + m_LongPathRequests.swap(longRequests); moveCount = (i32)longRequests.size(); } else { longRequests.resize(moveCount); - copy(m_AsyncLongPathRequests.begin(), m_AsyncLongPathRequests.begin() + moveCount, longRequests.begin()); - m_AsyncLongPathRequests.erase(m_AsyncLongPathRequests.begin(), m_AsyncLongPathRequests.begin() + moveCount); + copy(m_LongPathRequests.begin(), m_LongPathRequests.begin() + moveCount, longRequests.begin()); + m_LongPathRequests.erase(m_LongPathRequests.begin(), m_LongPathRequests.begin() + moveCount); } ProcessLongRequests(longRequests); m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); } - if (!m_AsyncShortPathRequests.empty()) + if (!m_ShortPathRequests.empty()) { // Figure out how many moves we can do now i32 moveCount = m_MaxSameTurnMoves - m_SameTurnMovesCount; if (moveCount <= 0) return; // Copy the short request elements we are going to process into a new array - std::vector shortRequests; - if ((i32)m_AsyncShortPathRequests.size() <= moveCount) + std::vector shortRequests; + if ((i32)m_ShortPathRequests.size() <= moveCount) { - m_AsyncShortPathRequests.swap(shortRequests); + m_ShortPathRequests.swap(shortRequests); moveCount = (i32)shortRequests.size(); } else { shortRequests.resize(moveCount); - copy(m_AsyncShortPathRequests.begin(), m_AsyncShortPathRequests.begin() + moveCount, shortRequests.begin()); - m_AsyncShortPathRequests.erase(m_AsyncShortPathRequests.begin(), m_AsyncShortPathRequests.begin() + moveCount); + copy(m_ShortPathRequests.begin(), m_ShortPathRequests.begin() + moveCount, shortRequests.begin()); + m_ShortPathRequests.erase(m_ShortPathRequests.begin(), m_ShortPathRequests.begin() + moveCount); } ProcessShortRequests(shortRequests); m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); } } ////////////////////////////////////////////////////////// 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) const { PROFILE2_IFSPIKE("Check Movement", 0.001); // Test against obstructions first. filter may discard pathfinding-blocking obstructions. // Use more permissive version of TestLine to allow unit-unit collisions to overlap slightly. // This makes movement smoother and more natural for units, overall. CmpPtr cmpObstructionManager(GetSystemEntity()); if (!cmpObstructionManager || cmpObstructionManager->TestLine(filter, x0, z0, x1, z1, r, true)) return false; // Then test against the terrain grid. This should not be necessary // But in case we allow terrain to change it will become so. return Pathfinding::CheckLineMovement(x0, z0, x1, z1, passClass, *m_TerrainOnlyGrid); } 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)) const { // Check unit obstruction CmpPtr 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; } 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) const { return CCmpPathfinder::CheckBuildingPlacement(filter, x, z, a, w, h, id, passClass, 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)) const { // Check unit obstruction CmpPtr 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 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 (const 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; } Index: ps/trunk/source/simulation2/components/CCmpPathfinder_Common.h =================================================================== --- ps/trunk/source/simulation2/components/CCmpPathfinder_Common.h (revision 22304) +++ ps/trunk/source/simulation2/components/CCmpPathfinder_Common.h (revision 22305) @@ -1,257 +1,257 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #ifndef INCLUDED_CCMPPATHFINDER_COMMON #define INCLUDED_CCMPPATHFINDER_COMMON /** * @file * Declares CCmpPathfinder. Its implementation is mainly done in CCmpPathfinder.cpp, * but the short-range (vertex) pathfinding is done in CCmpPathfinder_Vertex.cpp. * This file provides common code needed for both files. * * The long-range pathfinding is done by a LongPathfinder object. */ #include "simulation2/system/Component.h" #include "ICmpPathfinder.h" #include "graphics/Overlay.h" #include "graphics/Terrain.h" #include "maths/MathUtil.h" #include "ps/CLogger.h" #include "renderer/TerrainOverlay.h" #include "simulation2/components/ICmpObstructionManager.h" class HierarchicalPathfinder; class LongPathfinder; class VertexPathfinder; class SceneCollector; class AtlasOverlay; #ifdef NDEBUG #define PATHFIND_DEBUG 0 #else #define PATHFIND_DEBUG 1 #endif /** * Implementation of ICmpPathfinder */ class CCmpPathfinder final : public ICmpPathfinder { public: static void ClassInit(CComponentManager& componentManager) { componentManager.SubscribeToMessageType(MT_Update); componentManager.SubscribeToMessageType(MT_RenderSubmit); // for debug overlays componentManager.SubscribeToMessageType(MT_TerrainChanged); componentManager.SubscribeToMessageType(MT_WaterChanged); componentManager.SubscribeToMessageType(MT_ObstructionMapShapeChanged); componentManager.SubscribeToMessageType(MT_TurnStart); } ~CCmpPathfinder(); DEFAULT_COMPONENT_ALLOCATOR(Pathfinder) // Template state: std::map m_PassClassMasks; std::vector m_PassClasses; // Dynamic state: - std::vector m_AsyncLongPathRequests; - std::vector m_AsyncShortPathRequests; + std::vector m_LongPathRequests; + std::vector m_ShortPathRequests; u32 m_NextAsyncTicket; // unique IDs for asynchronous path requests u16 m_SameTurnMovesCount; // current number of same turn moves we have processed this turn // Lazily-constructed dynamic state (not serialized): u16 m_MapSize; // tiles per side Grid* m_Grid; // terrain/passability information Grid* m_TerrainOnlyGrid; // same as m_Grid, but only with terrain, to avoid some recomputations // Keep clever updates in memory to avoid memory fragmentation from the grid. // This should be used only in UpdateGrid(), there is no guarantee the data is properly initialized anywhere else. GridUpdateInformation m_DirtinessInformation; // The data from clever updates is stored for the AI manager GridUpdateInformation m_AIPathfinderDirtinessInformation; bool m_TerrainDirty; std::unique_ptr m_VertexPathfinder; std::unique_ptr m_PathfinderHier; std::unique_ptr m_LongPathfinder; // For responsiveness we will process some moves in the same turn they were generated in u16 m_MaxSameTurnMoves; // max number of moves that can be created and processed in the same turn AtlasOverlay* m_AtlasOverlay; static std::string GetSchema() { return ""; } virtual void Init(const CParamNode& paramNode); virtual void Deinit(); template void SerializeCommon(S& serialize); virtual void Serialize(ISerializer& serialize); virtual void Deserialize(const CParamNode& paramNode, IDeserializer& deserialize); virtual void HandleMessage(const CMessage& msg, bool global); virtual pass_class_t GetPassabilityClass(const std::string& name) const; virtual void GetPassabilityClasses(std::map& passClasses) const; virtual void GetPassabilityClasses( std::map& nonPathfindingPassClasses, std::map& pathfindingPassClasses) const; const PathfinderPassability* GetPassabilityFromMask(pass_class_t passClass) const; virtual entity_pos_t GetClearance(pass_class_t passClass) const { const PathfinderPassability* passability = GetPassabilityFromMask(passClass); if (!passability) return fixed::Zero(); return passability->m_Clearance; } virtual entity_pos_t GetMaximumClearance() const { entity_pos_t max = fixed::Zero(); for (const PathfinderPassability& passability : m_PassClasses) if (passability.m_Clearance > max) max = passability.m_Clearance; return max + Pathfinding::CLEARANCE_EXTENSION_RADIUS; } virtual const Grid& GetPassabilityGrid(); virtual const GridUpdateInformation& GetAIPathfinderDirtinessInformation() const { return m_AIPathfinderDirtinessInformation; } virtual void FlushAIPathfinderDirtinessInformation() { m_AIPathfinderDirtinessInformation.Clean(); } virtual Grid ComputeShoreGrid(bool expandOnWater = false); virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) const; virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify); - virtual WaypointPath ComputeShortPath(const AsyncShortPathRequest& request) const; + virtual WaypointPath ComputeShortPath(const ShortPathRequest& request) const; virtual u32 ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t controller, entity_id_t notify); virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass); virtual void SetDebugOverlay(bool enabled); virtual void SetHierDebugOverlay(bool enabled); virtual void GetDebugData(u32& steps, double& time, Grid& grid) const; virtual void SetAtlasOverlay(bool enable, pass_class_t passClass = 0); virtual bool 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) const; virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint) const; virtual ICmpObstruction::EFoundationCheck 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) const; virtual ICmpObstruction::EFoundationCheck 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) const; virtual void FinishAsyncRequests(); - void ProcessLongRequests(const std::vector& longRequests); + void ProcessLongRequests(const std::vector& longRequests); - void ProcessShortRequests(const std::vector& shortRequests); + void ProcessShortRequests(const std::vector& shortRequests); virtual void ProcessSameTurnMoves(); /** * Regenerates the grid based on the current obstruction list, if necessary */ virtual void UpdateGrid(); /** * Updates the terrain-only grid without updating the dirtiness informations. * Useful for fast passability updates in Atlas. */ void MinimalTerrainUpdate(); /** * Regenerates the terrain-only grid. * Atlas doesn't need to have passability cells expanded. */ void TerrainUpdateHelper(bool expandPassability = true); void RenderSubmit(SceneCollector& collector); }; class AtlasOverlay : public TerrainTextureOverlay { public: const CCmpPathfinder* m_Pathfinder; pass_class_t m_PassClass; AtlasOverlay(const CCmpPathfinder* pathfinder, pass_class_t passClass) : TerrainTextureOverlay(Pathfinding::NAVCELLS_PER_TILE), m_Pathfinder(pathfinder), m_PassClass(passClass) { } virtual void BuildTextureRGBA(u8* data, size_t w, size_t h) { // Render navcell passability, based on the terrain-only grid u8* p = data; for (size_t j = 0; j < h; ++j) { for (size_t i = 0; i < w; ++i) { SColor4ub color(0, 0, 0, 0); if (!IS_PASSABLE(m_Pathfinder->m_TerrainOnlyGrid->get((int)i, (int)j), m_PassClass)) color = SColor4ub(255, 0, 0, 127); *p++ = color.R; *p++ = color.G; *p++ = color.B; *p++ = color.A; } } } }; #endif // INCLUDED_CCMPPATHFINDER_COMMON Index: ps/trunk/source/simulation2/components/ICmpPathfinder.h =================================================================== --- ps/trunk/source/simulation2/components/ICmpPathfinder.h (revision 22304) +++ ps/trunk/source/simulation2/components/ICmpPathfinder.h (revision 22305) @@ -1,199 +1,199 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #ifndef INCLUDED_ICMPPATHFINDER #define INCLUDED_ICMPPATHFINDER #include "simulation2/system/Interface.h" #include "simulation2/components/ICmpObstruction.h" #include "simulation2/helpers/PathGoal.h" #include "simulation2/helpers/Pathfinding.h" #include "maths/FixedVector2D.h" #include #include class IObstructionTestFilter; template class Grid; /** * Pathfinder algorithms. * * There are two different modes: a tile-based pathfinder that works over long distances and * accounts for terrain costs but ignore units, and a 'short' vertex-based pathfinder that * provides precise paths and avoids other units. * * Both use the same concept of a PathGoal: either a point, circle or square. * (If the starting point is inside the goal shape then the path will move outwards * to reach the shape's outline.) * * The output is a list of waypoints. */ class ICmpPathfinder : public IComponent { public: /** * Get the list of all known passability classes. */ virtual void GetPassabilityClasses(std::map& passClasses) const = 0; /** * Get the list of passability classes, separating pathfinding classes and others. */ virtual void GetPassabilityClasses( std::map& nonPathfindingPassClasses, std::map& pathfindingPassClasses) const = 0; /** * Get the tag for a given passability class name. * Logs an error and returns something acceptable if the name is unrecognised. */ virtual pass_class_t GetPassabilityClass(const std::string& name) const = 0; virtual entity_pos_t GetClearance(pass_class_t passClass) const = 0; /** * Get the larger clearance in all passability classes. */ virtual entity_pos_t GetMaximumClearance() const = 0; virtual const Grid& GetPassabilityGrid() = 0; /** * Get the accumulated dirtiness information since the last time the AI accessed and flushed it. */ virtual const GridUpdateInformation& GetAIPathfinderDirtinessInformation() const = 0; virtual void FlushAIPathfinderDirtinessInformation() = 0; /** * Get a grid representing the distance to the shore of the terrain tile. */ virtual Grid ComputeShoreGrid(bool expandOnWater = false) = 0; /** * Compute a tile-based path from the given point to the goal, and return the set of waypoints. * The waypoints correspond to the centers of horizontally/vertically adjacent tiles * along the path. */ virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) const = 0; /** * Asynchronous version of ComputePath. * The result will be sent as CMessagePathResult to 'notify'. * Returns a unique non-zero number, which will match the 'ticket' in the result, * so callers can recognise each individual request they make. */ virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) = 0; /** * If the debug overlay is enabled, render the path that will computed by ComputePath. */ virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) = 0; /** * Compute a precise path from the given point to the goal, and return the set of waypoints. * The path is based on the full set of obstructions that pass the filter, such that * a unit of clearance 'clearance' will be able to follow the path with no collisions. * The path is restricted to a box of radius 'range' from the starting point. */ - virtual WaypointPath ComputeShortPath(const AsyncShortPathRequest& request) const = 0; + virtual WaypointPath ComputeShortPath(const ShortPathRequest& request) const = 0; /** * Asynchronous version of ComputeShortPath (using ControlGroupObstructionFilter). * The result will be sent as CMessagePathResult to 'notify'. * Returns a unique non-zero number, which will match the 'ticket' in the result, * so callers can recognise each individual request they make. */ virtual u32 ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t group, entity_id_t notify) = 0; /** * Check whether the given movement line is valid and doesn't hit any obstructions * or impassable terrain. * Returns true if the movement is okay. */ virtual bool 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) const = 0; /** * Check whether a unit placed here is valid and doesn't hit any obstructions * or impassable terrain. * When onlyCenterPoint = true, only check the center tile of the unit * @return ICmpObstruction::FOUNDATION_CHECK_SUCCESS if the placement is okay, else * a value describing the type of failure. */ virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint = false) const = 0; /** * Check whether a building placed here is valid and doesn't hit any obstructions * or impassable terrain. * @return ICmpObstruction::FOUNDATION_CHECK_SUCCESS if the placement is okay, else * a value describing the type of failure. */ virtual ICmpObstruction::EFoundationCheck 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) const = 0; /** * Check whether a building placed here is valid and doesn't hit any obstructions * or impassable terrain. * when onlyCenterPoint = true, only check the center tile of the building * @return ICmpObstruction::FOUNDATION_CHECK_SUCCESS if the placement is okay, else * a value describing the type of failure. */ virtual ICmpObstruction::EFoundationCheck 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) const = 0; /** * Toggle the storage and rendering of debug info. */ virtual void SetDebugOverlay(bool enabled) = 0; /** * Toggle the storage and rendering of debug info for the hierarchical pathfinder. */ virtual void SetHierDebugOverlay(bool enabled) = 0; /** * Finish computing asynchronous path requests and send the CMessagePathResult messages. */ virtual void FinishAsyncRequests() = 0; /** * Process moves during the same turn they were created in to improve responsiveness. */ virtual void ProcessSameTurnMoves() = 0; /** * Regenerates the grid based on the current obstruction list, if necessary */ virtual void UpdateGrid() = 0; /** * Returns some stats about the last ComputePath. */ virtual void GetDebugData(u32& steps, double& time, Grid& grid) const = 0; /** * Sets up the pathfinder passability overlay in Atlas. */ virtual void SetAtlasOverlay(bool enable, pass_class_t passClass = 0) = 0; DECLARE_INTERFACE_TYPE(Pathfinder) }; #endif // INCLUDED_ICMPPATHFINDER Index: ps/trunk/source/simulation2/components/tests/test_Pathfinder.h =================================================================== --- ps/trunk/source/simulation2/components/tests/test_Pathfinder.h (revision 22304) +++ ps/trunk/source/simulation2/components/tests/test_Pathfinder.h (revision 22305) @@ -1,427 +1,427 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #include "simulation2/system/ComponentTest.h" #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/components/ICmpPathfinder.h" #include "graphics/MapReader.h" #include "graphics/Terrain.h" #include "graphics/TerrainTextureManager.h" #include "lib/timer.h" #include "lib/tex/tex.h" #include "ps/Loader.h" #include "ps/Pyrogenesis.h" #include "simulation2/Simulation2.h" class TestCmpPathfinder : public CxxTest::TestSuite { public: void setUp() { g_VFS = CreateVfs(); g_VFS->Mount(L"", DataDir()/"mods"/"mod", VFS_MOUNT_MUST_EXIST); g_VFS->Mount(L"", DataDir()/"mods"/"public", VFS_MOUNT_MUST_EXIST, 1); // ignore directory-not-found errors TS_ASSERT_OK(g_VFS->Mount(L"cache", DataDir()/"_testcache")); CXeromyces::Startup(); // Need some stuff for terrain movement costs: // (TODO: this ought to be independent of any graphics code) new CTerrainTextureManager; g_TexMan.LoadTerrainTextures(); } void tearDown() { delete &g_TexMan; CXeromyces::Terminate(); g_VFS.reset(); DeleteDirectory(DataDir()/"_testcache"); } void test_namespace() { // Check that Pathfinding::NAVCELL_SIZE is actually an integer and that the definitions // of Pathfinding::NAVCELL_SIZE_INT and Pathfinding::NAVCELL_SIZE_LOG2 match TS_ASSERT_EQUALS(Pathfinding::NAVCELL_SIZE.ToInt_RoundToNegInfinity(), Pathfinding::NAVCELL_SIZE.ToInt_RoundToInfinity()); TS_ASSERT_EQUALS(Pathfinding::NAVCELL_SIZE.ToInt_RoundToNearest(), Pathfinding::NAVCELL_SIZE_INT); TS_ASSERT_EQUALS((Pathfinding::NAVCELL_SIZE >> 1).ToInt_RoundToZero(), Pathfinding::NAVCELL_SIZE_LOG2); } void test_pathgoal_nearest_distance() { entity_pos_t i = Pathfinding::NAVCELL_SIZE; CFixedVector2D u(i*1, i*0); CFixedVector2D v(i*0, i*1); { PathGoal goal = { PathGoal::POINT, i*8, i*6 }; TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*6); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*2); TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*8 + v*6); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*10); TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*12, i*9)); TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*8, i*6)); TS_ASSERT(goal.RectContainsGoal(i*8, i*6, i*12, i*9)); TS_ASSERT(!goal.RectContainsGoal(i*4, i*3, i*7, i*5)); TS_ASSERT(!goal.RectContainsGoal(i*9, i*7, i*13, i*15)); } { PathGoal goal = { PathGoal::CIRCLE, i*8, i*6, i*5 }; TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*4); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*0); TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*4 + v*3); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*5); TS_ASSERT(goal.RectContainsGoal(i*7, i*5, i*9, i*7)); // fully inside TS_ASSERT(goal.RectContainsGoal(i*3, i*1, i*13, i*11)); // fully outside TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*8, i*6)); // partially inside TS_ASSERT(goal.RectContainsGoal(i*4, i*0, i*12, i*1)); // touching the edge } { PathGoal goal = { PathGoal::INVERTED_CIRCLE, i*8, i*6, i*5 }; TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*1); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*3); TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*0 + v*0); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*0); TS_ASSERT(!goal.RectContainsGoal(i*7, i*5, i*9, i*7)); // fully inside TS_ASSERT(goal.RectContainsGoal(i*3, i*1, i*13, i*11)); // fully outside TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*8, i*6)); // partially inside TS_ASSERT(goal.RectContainsGoal(i*4, i*0, i*12, i*1)); // touching the edge } { PathGoal goal = { PathGoal::SQUARE, i*8, i*6, i*4, i*3, u, v }; TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*4); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*0); TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*4 + v*3); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*5); TS_ASSERT(goal.RectContainsGoal(i*7, i*5, i*9, i*7)); // fully inside TS_ASSERT(goal.RectContainsGoal(i*3, i*1, i*13, i*11)); // fully outside TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*8, i*6)); // partially inside TS_ASSERT(goal.RectContainsGoal(i*4, i*2, i*12, i*3)); // touching the edge TS_ASSERT(goal.RectContainsGoal(i*3, i*0, i*4, i*10)); // touching the edge } { PathGoal goal = { PathGoal::INVERTED_SQUARE, i*8, i*6, i*4, i*3, u, v }; TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*3); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*1); TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*0 + v*0); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*0); TS_ASSERT(!goal.RectContainsGoal(i*7, i*5, i*9, i*7)); // fully inside TS_ASSERT(goal.RectContainsGoal(i*3, i*1, i*13, i*11)); // fully outside TS_ASSERT(!goal.RectContainsGoal(i*4, i*3, i*8, i*6)); // inside, touching (should fail) TS_ASSERT(goal.RectContainsGoal(i*4, i*2, i*12, i*3)); // touching the edge TS_ASSERT(goal.RectContainsGoal(i*3, i*0, i*4, i*10)); // touching the edge } } void test_performance_DISABLED() { CTerrain terrain; CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); sim2.LoadDefaultScripts(); sim2.ResetState(); std::unique_ptr mapReader(new CMapReader()); LDR_BeginRegistering(); mapReader->LoadMap(L"maps/skirmishes/Median Oasis (2).pmp", sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue, &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL, &sim2, &sim2.GetSimContext(), -1, false); LDR_EndRegistering(); TS_ASSERT_OK(LDR_NonprogressiveLoad()); sim2.PreInitGame(); sim2.InitGame(); sim2.Update(0); CmpPtr cmp(sim2, SYSTEM_ENTITY); #if 0 entity_pos_t x0 = entity_pos_t::FromInt(10); entity_pos_t z0 = entity_pos_t::FromInt(495); entity_pos_t x1 = entity_pos_t::FromInt(500); entity_pos_t z1 = entity_pos_t::FromInt(495); ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, x1, z1 }; WaypointPath path; cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), path); for (size_t i = 0; i < path.m_Waypoints.size(); ++i) printf("%d: %f %f\n", (int)i, path.m_Waypoints[i].x.ToDouble(), path.m_Waypoints[i].z.ToDouble()); #endif double t = timer_Time(); srand(1234); for (size_t j = 0; j < 1024*2; ++j) { entity_pos_t x0 = entity_pos_t::FromInt(rand() % 512); entity_pos_t z0 = entity_pos_t::FromInt(rand() % 512); entity_pos_t x1 = x0 + entity_pos_t::FromInt(rand() % 64); entity_pos_t z1 = z0 + entity_pos_t::FromInt(rand() % 64); PathGoal goal = { PathGoal::POINT, x1, z1 }; WaypointPath path; cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), path); } t = timer_Time() - t; printf("[%f]", t); } void test_performance_short_DISABLED() { CTerrain terrain; terrain.Initialize(5, NULL); CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); sim2.LoadDefaultScripts(); sim2.ResetState(); const entity_pos_t range = entity_pos_t::FromInt(TERRAIN_TILE_SIZE*12); CmpPtr cmpObstructionMan(sim2, SYSTEM_ENTITY); CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY); srand(0); for (size_t i = 0; i < 200; ++i) { fixed x = fixed::FromFloat(1.5f*range.ToFloat() * rand()/(float)RAND_MAX); fixed z = fixed::FromFloat(1.5f*range.ToFloat() * rand()/(float)RAND_MAX); // printf("# %f %f\n", x.ToFloat(), z.ToFloat()); cmpObstructionMan->AddUnitShape(INVALID_ENTITY, x, z, fixed::FromInt(2), 0, INVALID_ENTITY); } PathGoal goal = { PathGoal::POINT, range, range }; - WaypointPath path = cmpPathfinder->ComputeShortPath(AsyncShortPathRequest{ 0, range/3, range/3, fixed::FromInt(2), range, goal, 0, false, 0, 0 }); + WaypointPath path = cmpPathfinder->ComputeShortPath(ShortPathRequest{ 0, range/3, range/3, fixed::FromInt(2), range, goal, 0, false, 0, 0 }); for (size_t i = 0; i < path.m_Waypoints.size(); ++i) printf("# %d: %f %f\n", (int)i, path.m_Waypoints[i].x.ToFloat(), path.m_Waypoints[i].z.ToFloat()); } template void DumpGrid(std::ostream& stream, const Grid& grid, int mask) { for (u16 j = 0; j < grid.m_H; ++j) { for (u16 i = 0; i < grid.m_W; ) { if (!(grid.get(i, j) & mask)) { i++; continue; } u16 i0 = i; for (i = i0+1; ; ++i) { if (i >= grid.m_W || !(grid.get(i, j) & mask)) { stream << " \n"; break; } } } } } void test_perf2_DISABLED() { CTerrain terrain; CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); sim2.LoadDefaultScripts(); sim2.ResetState(); std::unique_ptr mapReader(new CMapReader()); LDR_BeginRegistering(); mapReader->LoadMap(L"maps/scenarios/Peloponnese.pmp", sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue, &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL, &sim2, &sim2.GetSimContext(), -1, false); LDR_EndRegistering(); TS_ASSERT_OK(LDR_NonprogressiveLoad()); sim2.PreInitGame(); sim2.InitGame(); sim2.Update(0); std::ofstream stream(OsString("perf2.html").c_str(), std::ofstream::out | std::ofstream::trunc); CmpPtr cmpObstructionManager(sim2, SYSTEM_ENTITY); CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY); pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default"); const Grid& obstructions = cmpPathfinder->GetPassabilityGrid(); int scale = 1; stream << "\n"; stream << "\n"; stream << "\n"; stream << "\n"; stream << " \n"; DumpGrid(stream, obstructions, obstructionsMask); stream << " \n"; DumpPath(stream, 128*4, 256*4, 128*4, 384*4, cmpPathfinder); // RepeatPath(500, 128*4, 256*4, 128*4, 384*4, cmpPathfinder); // // DumpPath(stream, 128*4, 204*4, 192*4, 204*4, cmpPathfinder); // // DumpPath(stream, 128*4, 230*4, 32*4, 230*4, cmpPathfinder); stream << "\n"; stream << "\n"; } void test_perf3_DISABLED() { CTerrain terrain; CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); sim2.LoadDefaultScripts(); sim2.ResetState(); std::unique_ptr mapReader(new CMapReader()); LDR_BeginRegistering(); mapReader->LoadMap(L"maps/scenarios/Peloponnese.pmp", sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue, &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL, &sim2, &sim2.GetSimContext(), -1, false); LDR_EndRegistering(); TS_ASSERT_OK(LDR_NonprogressiveLoad()); sim2.PreInitGame(); sim2.InitGame(); sim2.Update(0); std::ofstream stream(OsString("perf3.html").c_str(), std::ofstream::out | std::ofstream::trunc); CmpPtr cmpObstructionManager(sim2, SYSTEM_ENTITY); CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY); pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default"); const Grid& obstructions = cmpPathfinder->GetPassabilityGrid(); int scale = 31; stream << "\n"; stream << "\n"; stream << "\n"; stream << "\n"; stream << "\n"; stream << "\n"; stream << "\n"; stream << " \n"; DumpGrid(stream, obstructions, obstructionsMask); stream << " \n"; for (int j = 160; j < 190; ++j) for (int i = 220; i < 290; ++i) DumpPath(stream, 230, 178, i, j, cmpPathfinder); stream << "\n"; stream << "\n"; stream << "\n"; } void DumpPath(std::ostream& stream, int i0, int j0, int i1, int j1, CmpPtr& cmpPathfinder) { entity_pos_t x0 = entity_pos_t::FromInt(i0); entity_pos_t z0 = entity_pos_t::FromInt(j0); entity_pos_t x1 = entity_pos_t::FromInt(i1); entity_pos_t z1 = entity_pos_t::FromInt(j1); PathGoal goal = { PathGoal::POINT, x1, z1 }; WaypointPath path; cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path); u32 debugSteps; double debugTime; Grid debugGrid; cmpPathfinder->GetDebugData(debugSteps, debugTime, debugGrid); // stream << " \n"; stream << " \n"; // stream << " \n"; // DumpGrid(stream, debugGrid, 1); // stream << " \n"; // stream << " \n"; // DumpGrid(stream, debugGrid, 2); // stream << " \n"; // stream << " \n"; // DumpGrid(stream, debugGrid, 3); // stream << " \n"; stream << " \n"; stream << " \n"; } void RepeatPath(int n, int i0, int j0, int i1, int j1, CmpPtr& cmpPathfinder) { entity_pos_t x0 = entity_pos_t::FromInt(i0); entity_pos_t z0 = entity_pos_t::FromInt(j0); entity_pos_t x1 = entity_pos_t::FromInt(i1); entity_pos_t z1 = entity_pos_t::FromInt(j1); PathGoal goal = { PathGoal::POINT, x1, z1 }; double t = timer_Time(); for (int i = 0; i < n; ++i) { WaypointPath path; cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path); } t = timer_Time() - t; debug_printf("### RepeatPath %fms each (%fs total)\n", 1000*t / n, t); } }; Index: ps/trunk/source/simulation2/helpers/Pathfinding.h =================================================================== --- ps/trunk/source/simulation2/helpers/Pathfinding.h (revision 22304) +++ ps/trunk/source/simulation2/helpers/Pathfinding.h (revision 22305) @@ -1,409 +1,409 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #ifndef INCLUDED_PATHFINDING #define INCLUDED_PATHFINDING #include "maths/MathUtil.h" #include "ps/CLogger.h" #include "simulation2/system/Entity.h" #include "simulation2/system/ParamNode.h" #include "graphics/Terrain.h" #include "Grid.h" #include "PathGoal.h" typedef u16 pass_class_t; -struct AsyncLongPathRequest +struct LongPathRequest { u32 ticket; entity_pos_t x0; entity_pos_t z0; PathGoal goal; pass_class_t passClass; entity_id_t notify; }; -struct AsyncShortPathRequest +struct ShortPathRequest { u32 ticket; entity_pos_t x0; entity_pos_t z0; entity_pos_t clearance; entity_pos_t range; PathGoal goal; pass_class_t passClass; bool avoidMovingUnits; entity_id_t group; entity_id_t notify; }; struct Waypoint { entity_pos_t x, z; }; /** * Returned path. * Waypoints are in *reverse* order (the earliest is at the back of the list) */ struct WaypointPath { std::vector m_Waypoints; }; /** * Represents the cost of a path consisting of horizontal/vertical and * diagonal movements over a uniform-cost grid. * Maximum path length before overflow is about 45K steps. */ struct PathCost { PathCost() : data(0) { } /// Construct from a number of horizontal/vertical and diagonal steps PathCost(u16 hv, u16 d) : data(hv * 65536 + d * 92682) // 2^16 * sqrt(2) == 92681.9 { } /// Construct for horizontal/vertical movement of given number of steps static PathCost horizvert(u16 n) { return PathCost(n, 0); } /// Construct for diagonal movement of given number of steps static PathCost diag(u16 n) { return PathCost(0, n); } PathCost operator+(const PathCost& a) const { PathCost c; c.data = data + a.data; return c; } PathCost& operator+=(const PathCost& a) { data += a.data; return *this; } bool operator<=(const PathCost& b) const { return data <= b.data; } bool operator< (const PathCost& b) const { return data < b.data; } bool operator>=(const PathCost& b) const { return data >= b.data; } bool operator>(const PathCost& b) const { return data > b.data; } u32 ToInt() { return data; } private: u32 data; }; static const int PASS_CLASS_BITS = 16; typedef u16 NavcellData; // 1 bit per passability class (up to PASS_CLASS_BITS) #define IS_PASSABLE(item, classmask) (((item) & (classmask)) == 0) #define PASS_CLASS_MASK_FROM_INDEX(id) ((pass_class_t)(1u << id)) #define SPECIAL_PASS_CLASS PASS_CLASS_MASK_FROM_INDEX((PASS_CLASS_BITS-1)) // 16th bit, used for special in-place computations namespace Pathfinding { /** * The long-range pathfinder operates primarily over a navigation grid (a uniform-cost * 2D passability grid, with horizontal/vertical (not diagonal) connectivity). * This is based on the terrain tile passability, plus the rasterized shapes of * obstructions, all expanded outwards by the radius of the units. * Since units are much smaller than terrain tiles, the nav grid should be * higher resolution than the tiles. * We therefore split each terrain tile into NxN "nav cells" (for some integer N, * preferably a power of two). */ const int NAVCELLS_PER_TILE = 4; /** * Size of a navcell in metres ( = TERRAIN_TILE_SIZE / NAVCELLS_PER_TILE) */ const fixed NAVCELL_SIZE = fixed::FromInt((int)TERRAIN_TILE_SIZE) / Pathfinding::NAVCELLS_PER_TILE; const int NAVCELL_SIZE_INT = 1; const int NAVCELL_SIZE_LOG2 = 0; /** * For extending the goal outwards/inwards a little bit * NOTE: keep next to the definition of NAVCELL_SIZE to avoid init order problems * between translation units. * TODO: figure out whether this is actually needed. It was added back in r8751 (in 2010) for unclear reasons * and it does not seem to really improve behavior today */ const entity_pos_t GOAL_DELTA = NAVCELL_SIZE/8; /** * To make sure the long-range pathfinder is more strict than the short-range one, * we need to slightly over-rasterize. So we extend the clearance radius by 1. */ const entity_pos_t CLEARANCE_EXTENSION_RADIUS = fixed::FromInt(1); /** * Compute the navcell indexes on the grid nearest to a given point * w, h are the grid dimensions, i.e. the number of navcells per side */ inline void NearestNavcell(entity_pos_t x, entity_pos_t z, u16& i, u16& j, u16 w, u16 h) { // Use NAVCELL_SIZE_INT to save the cost of dividing by a fixed i = (u16)clamp((x / NAVCELL_SIZE_INT).ToInt_RoundToNegInfinity(), 0, w - 1); j = (u16)clamp((z / NAVCELL_SIZE_INT).ToInt_RoundToNegInfinity(), 0, h - 1); } /** * Returns the position of the center of the given tile */ inline void TileCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z) { cassert(TERRAIN_TILE_SIZE % 2 == 0); x = entity_pos_t::FromInt(i*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE / 2); z = entity_pos_t::FromInt(j*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE / 2); } inline void NavcellCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z) { x = entity_pos_t::FromInt(i * 2 + 1).Multiply(NAVCELL_SIZE / 2); z = entity_pos_t::FromInt(j * 2 + 1).Multiply(NAVCELL_SIZE / 2); } /* * Checks that the line (x0,z0)-(x1,z1) does not intersect any impassable navcells. */ inline bool CheckLineMovement(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, pass_class_t passClass, const Grid& grid) { // We shouldn't allow lines between diagonally-adjacent navcells. // It doesn't matter whether we allow lines precisely along the edge // of an impassable navcell. // To rasterise the line: // If the line is (e.g.) aiming up-right, then we start at the navcell // containing the start point and the line must either end in that navcell // or else exit along the top edge or the right edge (or through the top-right corner, // which we'll arbitrary treat as the horizontal edge). // So we jump into the adjacent navcell across that edge, and continue. // To handle the special case of units that are stuck on impassable cells, // we allow them to move from an impassable to a passable cell (but not // vice versa). u16 i0, j0, i1, j1; NearestNavcell(x0, z0, i0, j0, grid.m_W, grid.m_H); NearestNavcell(x1, z1, i1, j1, grid.m_W, grid.m_H); // Find which direction the line heads in int di = (i0 < i1 ? +1 : i1 < i0 ? -1 : 0); int dj = (j0 < j1 ? +1 : j1 < j0 ? -1 : 0); u16 i = i0; u16 j = j0; bool currentlyOnImpassable = !IS_PASSABLE(grid.get(i0, j0), passClass); while (true) { // Make sure we are still in the limits ENSURE( ((di > 0 && i0 <= i && i <= i1) || (di < 0 && i1 <= i && i <= i0) || (di == 0 && i == i0)) && ((dj > 0 && j0 <= j && j <= j1) || (dj < 0 && j1 <= j && j <= j0) || (dj == 0 && j == j0))); // Fail if we're moving onto an impassable navcell bool passable = IS_PASSABLE(grid.get(i, j), passClass); if (passable) currentlyOnImpassable = false; else if (!currentlyOnImpassable) return false; // Succeed if we're at the target if (i == i1 && j == j1) return true; // If we can only move horizontally/vertically, then just move in that direction // If we are reaching the limits, we can go straight to the end if (di == 0 || i == i1) { j += dj; continue; } else if (dj == 0 || j == j1) { i += di; continue; } // Otherwise we need to check which cell to move into: // Check whether the line intersects the horizontal (top/bottom) edge of // the current navcell. // Horizontal edge is (i, j + (dj>0?1:0)) .. (i + 1, j + (dj>0?1:0)) // Since we already know the line is moving from this navcell into a different // navcell, we simply need to test that the edge's endpoints are not both on the // same side of the line. // If we are crossing exactly a vertex of the grid, we will get dota or dotb equal // to 0. In that case we arbitrarily choose to move of dj. // This only works because we handle the case (i == i1 || j == j1) beforehand. // Otherwise we could go outside the j limits and never reach the final navcell. entity_pos_t xia = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); entity_pos_t xib = entity_pos_t::FromInt(i+1).Multiply(Pathfinding::NAVCELL_SIZE); entity_pos_t zj = entity_pos_t::FromInt(j + (dj+1)/2).Multiply(Pathfinding::NAVCELL_SIZE); CFixedVector2D perp = CFixedVector2D(x1 - x0, z1 - z0).Perpendicular(); entity_pos_t dota = (CFixedVector2D(xia, zj) - CFixedVector2D(x0, z0)).Dot(perp); entity_pos_t dotb = (CFixedVector2D(xib, zj) - CFixedVector2D(x0, z0)).Dot(perp); // If the horizontal edge is fully on one side of the line, so the line doesn't // intersect it, we should move across the vertical edge instead if ((dota < entity_pos_t::Zero() && dotb < entity_pos_t::Zero()) || (dota > entity_pos_t::Zero() && dotb > entity_pos_t::Zero())) i += di; else j += dj; } } } /* * For efficient pathfinding we want to try hard to minimise the per-tile search cost, * so we precompute the tile passability flags and movement costs for the various different * types of unit. * We also want to minimise memory usage (there can easily be 100K tiles so we don't want * to store many bytes for each). * * To handle passability efficiently, we have a small number of passability classes * (e.g. "infantry", "ship"). Each unit belongs to a single passability class, and * uses that for all its pathfinding. * Passability is determined by water depth, terrain slope, forestness, buildingness. * We need at least one bit per class per tile to represent passability. * * Not all pass classes are used for actual pathfinding. The pathfinder calls * CCmpObstructionManager's Rasterize() to add shapes onto the passability grid. * Which shapes are rasterized depend on the value of the m_Obstructions of each passability * class. * * Passabilities not used for unit pathfinding should not use the Clearance attribute, and * will get a zero clearance value. */ class PathfinderPassability { public: PathfinderPassability(pass_class_t mask, const CParamNode& node) : m_Mask(mask) { if (node.GetChild("MinWaterDepth").IsOk()) m_MinDepth = node.GetChild("MinWaterDepth").ToFixed(); else m_MinDepth = std::numeric_limits::min(); if (node.GetChild("MaxWaterDepth").IsOk()) m_MaxDepth = node.GetChild("MaxWaterDepth").ToFixed(); else m_MaxDepth = std::numeric_limits::max(); if (node.GetChild("MaxTerrainSlope").IsOk()) m_MaxSlope = node.GetChild("MaxTerrainSlope").ToFixed(); else m_MaxSlope = std::numeric_limits::max(); if (node.GetChild("MinShoreDistance").IsOk()) m_MinShore = node.GetChild("MinShoreDistance").ToFixed(); else m_MinShore = std::numeric_limits::min(); if (node.GetChild("MaxShoreDistance").IsOk()) m_MaxShore = node.GetChild("MaxShoreDistance").ToFixed(); else m_MaxShore = std::numeric_limits::max(); if (node.GetChild("Clearance").IsOk()) { m_Clearance = node.GetChild("Clearance").ToFixed(); /* According to Philip who designed the original doc (in docs/pathfinder.pdf), * clearance should usually be integer to ensure consistent behavior when rasterizing * the passability map. * This seems doubtful to me and my pathfinder fix makes having a clearance of 0.8 quite convenient * so I comment out this check, but leave it here for the purpose of documentation should a bug arise. if (!(m_Clearance % Pathfinding::NAVCELL_SIZE).IsZero()) { // If clearance isn't an integer number of navcells then we'll // probably get weird behaviour when expanding the navcell grid // by clearance, vs expanding static obstructions by clearance LOGWARNING("Pathfinder passability class has clearance %f, should be multiple of %f", m_Clearance.ToFloat(), Pathfinding::NAVCELL_SIZE.ToFloat()); }*/ } else m_Clearance = fixed::Zero(); if (node.GetChild("Obstructions").IsOk()) { std::wstring obstructions = node.GetChild("Obstructions").ToString(); if (obstructions == L"none") m_Obstructions = NONE; else if (obstructions == L"pathfinding") m_Obstructions = PATHFINDING; else if (obstructions == L"foundation") m_Obstructions = FOUNDATION; else { LOGERROR("Invalid value for Obstructions in pathfinder.xml for pass class %d", mask); m_Obstructions = NONE; } } else m_Obstructions = NONE; } bool IsPassable(fixed waterdepth, fixed steepness, fixed shoredist) { return ((m_MinDepth <= waterdepth && waterdepth <= m_MaxDepth) && (steepness < m_MaxSlope) && (m_MinShore <= shoredist && shoredist <= m_MaxShore)); } pass_class_t m_Mask; fixed m_Clearance; // min distance from static obstructions enum ObstructionHandling { NONE, PATHFINDING, FOUNDATION }; ObstructionHandling m_Obstructions; private: fixed m_MinDepth; fixed m_MaxDepth; fixed m_MaxSlope; fixed m_MinShore; fixed m_MaxShore; }; #endif // INCLUDED_PATHFINDING Index: ps/trunk/source/simulation2/helpers/VertexPathfinder.cpp =================================================================== --- ps/trunk/source/simulation2/helpers/VertexPathfinder.cpp (revision 22304) +++ ps/trunk/source/simulation2/helpers/VertexPathfinder.cpp (revision 22305) @@ -1,963 +1,963 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ /** * @file * Vertex-based algorithm for CCmpPathfinder. * Computes paths around the corners of rectangular obstructions. * * Useful search term for this algorithm: "points of visibility". * * Since we sometimes want to use this for avoiding moving units, there is no * pre-computation - the whole visibility graph is effectively regenerated for * each path, and it does A* over that graph. * * This scales very poorly in the number of obstructions, so it should be used * with a limited range and not exceedingly frequently. */ #include "precompiled.h" #include "VertexPathfinder.h" #include "lib/timer.h" #include "ps/Profile.h" #include "renderer/Scene.h" #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/helpers/PriorityQueue.h" #include "simulation2/helpers/Render.h" #include "simulation2/system/SimContext.h" /* Quadrant optimisation: * (loosely based on GPG2 "Optimizing Points-of-Visibility Pathfinding") * * Consider the vertex ("@") at a corner of an axis-aligned rectangle ("#"): * * TL : TR * : * ####@ - - - * ##### * ##### * BL ## BR * * The area around the vertex is split into TopLeft, BottomRight etc quadrants. * * If the shortest path reaches this vertex, it cannot continue to a vertex in * the BL quadrant (it would be blocked by the shape). * Since the shortest path is wrapped tightly around the edges of obstacles, * if the path approached this vertex from the TL quadrant, * it cannot continue to the TL or TR quadrants (the path could be shorter if it * skipped this vertex). * Therefore it must continue to a vertex in the BR quadrant (so this vertex is in * *that* vertex's TL quadrant). * * That lets us significantly reduce the search space by quickly discarding vertexes * from the wrong quadrants. * * (This causes badness if the path starts from inside the shape, so we add some hacks * for that case.) * * (For non-axis-aligned rectangles it's harder to do this computation, so we'll * not bother doing any discarding for those.) */ static const u8 QUADRANT_NONE = 0; static const u8 QUADRANT_BL = 1; static const u8 QUADRANT_TR = 2; static const u8 QUADRANT_TL = 4; static const u8 QUADRANT_BR = 8; static const u8 QUADRANT_BLTR = QUADRANT_BL|QUADRANT_TR; static const u8 QUADRANT_TLBR = QUADRANT_TL|QUADRANT_BR; static const u8 QUADRANT_ALL = QUADRANT_BLTR|QUADRANT_TLBR; // When computing vertexes to insert into the search graph, // add a small delta so that the vertexes of an edge don't get interpreted // as crossing the edge (given minor numerical inaccuracies) static const entity_pos_t EDGE_EXPAND_DELTA = entity_pos_t::FromInt(1)/16; /** * Check whether a ray from 'a' to 'b' crosses any of the edges. * (Edges are one-sided so it's only considered a cross if going from front to back.) */ inline static bool CheckVisibility(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) { CFixedVector2D abn = (b - a).Perpendicular(); // Edges of general non-axis-aligned shapes for (size_t i = 0; i < edges.size(); ++i) { CFixedVector2D p0 = edges[i].p0; CFixedVector2D p1 = edges[i].p1; CFixedVector2D d = (p1 - p0).Perpendicular(); // If 'a' is behind the edge, we can't cross fixed q = (a - p0).Dot(d); if (q < fixed::Zero()) continue; // If 'b' is in front of the edge, we can't cross fixed r = (b - p0).Dot(d); if (r > fixed::Zero()) continue; // The ray is crossing the infinitely-extended edge from in front to behind. // Check the finite edge is crossing the infinitely-extended ray too. // (Given the previous tests, it can only be crossing in one direction.) fixed s = (p0 - a).Dot(abn); if (s > fixed::Zero()) continue; fixed t = (p1 - a).Dot(abn); if (t < fixed::Zero()) continue; return false; } return true; } // Handle the axis-aligned shape edges separately (for performance): // (These are specialised versions of the general unaligned edge code. // They assume the caller has already excluded edges for which 'a' is // on the wrong side.) inline static bool CheckVisibilityLeft(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) { if (a.X >= b.X) return true; CFixedVector2D abn = (b - a).Perpendicular(); for (size_t i = 0; i < edges.size(); ++i) { if (b.X < edges[i].p0.X) continue; CFixedVector2D p0 (edges[i].p0.X, edges[i].c1); fixed s = (p0 - a).Dot(abn); if (s > fixed::Zero()) continue; CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y); fixed t = (p1 - a).Dot(abn); if (t < fixed::Zero()) continue; return false; } return true; } inline static bool CheckVisibilityRight(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) { if (a.X <= b.X) return true; CFixedVector2D abn = (b - a).Perpendicular(); for (size_t i = 0; i < edges.size(); ++i) { if (b.X > edges[i].p0.X) continue; CFixedVector2D p0 (edges[i].p0.X, edges[i].c1); fixed s = (p0 - a).Dot(abn); if (s > fixed::Zero()) continue; CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y); fixed t = (p1 - a).Dot(abn); if (t < fixed::Zero()) continue; return false; } return true; } inline static bool CheckVisibilityBottom(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) { if (a.Y >= b.Y) return true; CFixedVector2D abn = (b - a).Perpendicular(); for (size_t i = 0; i < edges.size(); ++i) { if (b.Y < edges[i].p0.Y) continue; CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y); fixed s = (p0 - a).Dot(abn); if (s > fixed::Zero()) continue; CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y); fixed t = (p1 - a).Dot(abn); if (t < fixed::Zero()) continue; return false; } return true; } inline static bool CheckVisibilityTop(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) { if (a.Y <= b.Y) return true; CFixedVector2D abn = (b - a).Perpendicular(); for (size_t i = 0; i < edges.size(); ++i) { if (b.Y > edges[i].p0.Y) continue; CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y); fixed s = (p0 - a).Dot(abn); if (s > fixed::Zero()) continue; CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y); fixed t = (p1 - a).Dot(abn); if (t < fixed::Zero()) continue; return false; } return true; } typedef PriorityQueueHeap VertexPriorityQueue; /** * Add edges and vertexes to represent the boundaries between passable and impassable * navcells (for impassable terrain). * Navcells i0 <= i <= i1, j0 <= j <= j1 will be considered. */ static void AddTerrainEdges(std::vector& edges, std::vector& vertexes, int i0, int j0, int i1, int j1, pass_class_t passClass, const Grid& grid) { // Clamp the coordinates so we won't attempt to sample outside of the grid. // (This assumes the outermost ring of navcells (which are always impassable) // won't have a boundary with any passable navcells. TODO: is that definitely // safe enough?) i0 = clamp(i0, 1, grid.m_W-2); j0 = clamp(j0, 1, grid.m_H-2); i1 = clamp(i1, 1, grid.m_W-2); j1 = clamp(j1, 1, grid.m_H-2); for (int j = j0; j <= j1; ++j) { for (int i = i0; i <= i1; ++i) { if (IS_PASSABLE(grid.get(i, j), passClass)) continue; if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i+1, j+1), passClass)) { Vertex vert; vert.status = Vertex::UNEXPLORED; vert.quadOutward = QUADRANT_ALL; vert.quadInward = QUADRANT_BL; vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); vertexes.push_back(vert); } if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i-1, j+1), passClass)) { Vertex vert; vert.status = Vertex::UNEXPLORED; vert.quadOutward = QUADRANT_ALL; vert.quadInward = QUADRANT_BR; vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); vertexes.push_back(vert); } if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i+1, j-1), passClass)) { Vertex vert; vert.status = Vertex::UNEXPLORED; vert.quadOutward = QUADRANT_ALL; vert.quadInward = QUADRANT_TL; vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); vertexes.push_back(vert); } if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i-1, j-1), passClass)) { Vertex vert; vert.status = Vertex::UNEXPLORED; vert.quadOutward = QUADRANT_ALL; vert.quadInward = QUADRANT_TR; vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); vertexes.push_back(vert); } } } // XXX rewrite this stuff std::vector segmentsR; std::vector segmentsL; for (int j = j0; j < j1; ++j) { segmentsR.clear(); segmentsL.clear(); for (int i = i0; i <= i1; ++i) { bool a = IS_PASSABLE(grid.get(i, j+1), passClass); bool b = IS_PASSABLE(grid.get(i, j), passClass); if (a && !b) segmentsL.push_back(i); if (b && !a) segmentsR.push_back(i); } if (!segmentsR.empty()) { segmentsR.push_back(0); // sentinel value to simplify the loop u16 ia = segmentsR[0]; u16 ib = ia + 1; for (size_t n = 1; n < segmentsR.size(); ++n) { if (segmentsR[n] == ib) ++ib; else { CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); edges.emplace_back(Edge{ v0, v1 }); ia = segmentsR[n]; ib = ia + 1; } } } if (!segmentsL.empty()) { segmentsL.push_back(0); // sentinel value to simplify the loop u16 ia = segmentsL[0]; u16 ib = ia + 1; for (size_t n = 1; n < segmentsL.size(); ++n) { if (segmentsL[n] == ib) ++ib; else { CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); edges.emplace_back(Edge{ v0, v1 }); ia = segmentsL[n]; ib = ia + 1; } } } } std::vector segmentsU; std::vector segmentsD; for (int i = i0; i < i1; ++i) { segmentsU.clear(); segmentsD.clear(); for (int j = j0; j <= j1; ++j) { bool a = IS_PASSABLE(grid.get(i+1, j), passClass); bool b = IS_PASSABLE(grid.get(i, j), passClass); if (a && !b) segmentsU.push_back(j); if (b && !a) segmentsD.push_back(j); } if (!segmentsU.empty()) { segmentsU.push_back(0); // sentinel value to simplify the loop u16 ja = segmentsU[0]; u16 jb = ja + 1; for (size_t n = 1; n < segmentsU.size(); ++n) { if (segmentsU[n] == jb) ++jb; else { CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE); CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE); edges.emplace_back(Edge{ v0, v1 }); ja = segmentsU[n]; jb = ja + 1; } } } if (!segmentsD.empty()) { segmentsD.push_back(0); // sentinel value to simplify the loop u16 ja = segmentsD[0]; u16 jb = ja + 1; for (size_t n = 1; n < segmentsD.size(); ++n) { if (segmentsD[n] == jb) ++jb; else { CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE); CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE); edges.emplace_back(Edge{ v0, v1 }); ja = segmentsD[n]; jb = ja + 1; } } } } } static void SplitAAEdges(const CFixedVector2D& a, const std::vector& edges, const std::vector& squares, std::vector& edgesUnaligned, std::vector& edgesLeft, std::vector& edgesRight, std::vector& edgesBottom, std::vector& edgesTop) { for (const Square& square : squares) { if (a.X <= square.p0.X) edgesLeft.emplace_back(EdgeAA{ square.p0, square.p1.Y }); if (a.X >= square.p1.X) edgesRight.emplace_back(EdgeAA{ square.p1, square.p0.Y }); if (a.Y <= square.p0.Y) edgesBottom.emplace_back(EdgeAA{ square.p0, square.p1.X }); if (a.Y >= square.p1.Y) edgesTop.emplace_back(EdgeAA{ square.p1, square.p0.X }); } for (const Edge& edge : edges) { if (edge.p0.X == edge.p1.X) { if (edge.p1.Y < edge.p0.Y) { if (!(a.X <= edge.p0.X)) continue; edgesLeft.emplace_back(EdgeAA{ edge.p1, edge.p0.Y }); } else { if (!(a.X >= edge.p0.X)) continue; edgesRight.emplace_back(EdgeAA{ edge.p1, edge.p0.Y }); } } else if (edge.p0.Y == edge.p1.Y) { if (edge.p0.X < edge.p1.X) { if (!(a.Y <= edge.p0.Y)) continue; edgesBottom.emplace_back(EdgeAA{ edge.p0, edge.p1.X }); } else { if (!(a.Y >= edge.p0.Y)) continue; edgesTop.emplace_back(EdgeAA{ edge.p0, edge.p1.X }); } } else edgesUnaligned.push_back(edge); } } /** * Functor for sorting edge-squares by approximate proximity to a fixed point. */ struct SquareSort { CFixedVector2D src; SquareSort(CFixedVector2D src) : src(src) { } bool operator()(const Square& a, const Square& b) const { if ((a.p0 - src).CompareLength(b.p0 - src) < 0) return true; return false; } }; -WaypointPath VertexPathfinder::ComputeShortPath(const AsyncShortPathRequest& request, CmpPtr cmpObstructionManager) const +WaypointPath VertexPathfinder::ComputeShortPath(const ShortPathRequest& request, CmpPtr cmpObstructionManager) const { PROFILE2("ComputeShortPath"); DebugRenderGoal(cmpObstructionManager->GetSimContext(), request.goal); // Create impassable edges at the max-range boundary, so we can't escape the region // where we're meant to be searching fixed rangeXMin = request.x0 - request.range; fixed rangeXMax = request.x0 + request.range; fixed rangeZMin = request.z0 - request.range; fixed rangeZMax = request.z0 + request.range; // we don't actually add the "search space" edges as edges, since we may want to cross them // in some cases (such as if we need to go around an obstruction that's partly out of the search range) // Add the start point to the graph CFixedVector2D posStart(request.x0, request.z0); fixed hStart = (posStart - request.goal.NearestPointOnGoal(posStart)).Length(); Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL }; m_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(request.goal.x, request.goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL }; m_Vertexes.push_back(end); const size_t GOAL_VERTEX_ID = 1; // Find all the obstruction squares that might affect us std::vector squares; size_t staticShapesNb = 0; ControlGroupMovementObstructionFilter filter(request.avoidMovingUnits, request.group); cmpObstructionManager->GetStaticObstructionsInRange(filter, rangeXMin - request.clearance, rangeZMin - request.clearance, rangeXMax + request.clearance, rangeZMax + request.clearance, squares); staticShapesNb = squares.size(); cmpObstructionManager->GetUnitObstructionsInRange(filter, rangeXMin - request.clearance, rangeZMin - request.clearance, rangeXMax + request.clearance, rangeZMax + request.clearance, squares); // Change array capacities to reduce reallocations m_Vertexes.reserve(m_Vertexes.size() + squares.size()*4); m_EdgeSquares.reserve(m_EdgeSquares.size() + squares.size()); // (assume most squares are AA) entity_pos_t pathfindClearance = request.clearance; // 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; if (i >= staticShapesNb) pathfindClearance = request.clearance - entity_pos_t::FromInt(1)/2; // Expand the vertexes by the moving unit's collision radius, to find the // closest we can get to it CFixedVector2D hd0(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA, squares[i].hh + pathfindClearance + EDGE_EXPAND_DELTA); CFixedVector2D hd1(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA, -(squares[i].hh + pathfindClearance + 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; m_Vertexes.push_back(vert); if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_TR; m_Vertexes.push_back(vert); if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_TL; m_Vertexes.push_back(vert); if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; m_Vertexes.push_back(vert); if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; // Add the edges: CFixedVector2D h0(squares[i].hw + pathfindClearance, squares[i].hh + pathfindClearance); CFixedVector2D h1(squares[i].hw + pathfindClearance, -(squares[i].hh + pathfindClearance)); 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) m_EdgeSquares.emplace_back(Square{ ev1, ev3 }); else { m_Edges.emplace_back(Edge{ ev0, ev1 }); m_Edges.emplace_back(Edge{ ev1, ev2 }); m_Edges.emplace_back(Edge{ ev2, ev3 }); m_Edges.emplace_back(Edge{ ev3, ev0 }); } // TODO: should clip out vertexes and edges that are outside the range, // to reduce the search space } // Add terrain obstructions { u16 i0, j0, i1, j1; Pathfinding::NearestNavcell(rangeXMin, rangeZMin, i0, j0, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); Pathfinding::NearestNavcell(rangeXMax, rangeZMax, i1, j1, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); AddTerrainEdges(m_Edges, m_Vertexes, i0, j0, i1, j1, request.passClass, *m_TerrainOnlyGrid); } // Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable) for (size_t i = 0; i < m_EdgeSquares.size(); ++i) { // If the start point is inside the square, ignore it if (start.p.X >= m_EdgeSquares[i].p0.X && start.p.Y >= m_EdgeSquares[i].p0.Y && start.p.X <= m_EdgeSquares[i].p1.X && start.p.Y <= m_EdgeSquares[i].p1.Y) continue; // Remove every non-start/goal vertex that is inside an edgeSquare; // since remove() would be inefficient, just mark it as closed instead. for (size_t j = 2; j < m_Vertexes.size(); ++j) if (m_Vertexes[j].p.X >= m_EdgeSquares[i].p0.X && m_Vertexes[j].p.Y >= m_EdgeSquares[i].p0.Y && m_Vertexes[j].p.X <= m_EdgeSquares[i].p1.X && m_Vertexes[j].p.Y <= m_EdgeSquares[i].p1.Y) m_Vertexes[j].status = Vertex::CLOSED; } ENSURE(m_Vertexes.size() < 65536); // We store array indexes as u16. DebugRenderGraph(cmpObstructionManager->GetSimContext(), m_Vertexes, m_Edges, m_EdgeSquares); // 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. VertexPriorityQueue open; VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h }; open.push(qiStart); u16 idBest = START_VERTEX_ID; fixed hBest = start.h; while (!open.empty()) { // Move best tile from open to closed VertexPriorityQueue::Item curr = open.pop(); m_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 by distance in order to check those first that have a high probability of blocking a ray. // The heuristic based on distance is very rough, especially for squares that are further away; // we're also only really interested in the closest squares since they are the only ones that block a lot of rays. // Thus we only do a partial sort; the threshold is just a somewhat reasonable value. if (m_EdgeSquares.size() > 8) std::partial_sort(m_EdgeSquares.begin(), m_EdgeSquares.begin() + 8, m_EdgeSquares.end(), SquareSort(m_Vertexes[curr.id].p)); m_EdgesUnaligned.clear(); m_EdgesLeft.clear(); m_EdgesRight.clear(); m_EdgesBottom.clear(); m_EdgesTop.clear(); SplitAAEdges(m_Vertexes[curr.id].p, m_Edges, m_EdgeSquares, m_EdgesUnaligned, m_EdgesLeft, m_EdgesRight, m_EdgesBottom, m_EdgesTop); // Check the lines to every other vertex for (size_t n = 0; n < m_Vertexes.size(); ++n) { if (m_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 = request.goal.NearestPointOnGoal(m_Vertexes[curr.id].p); // 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 = m_Vertexes[n].p; // Work out which quadrant(s) we're approaching the new vertex from u8 quad = 0; if (m_Vertexes[curr.id].p.X <= npos.X && m_Vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL; if (m_Vertexes[curr.id].p.X >= npos.X && m_Vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR; if (m_Vertexes[curr.id].p.X <= npos.X && m_Vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL; if (m_Vertexes[curr.id].p.X >= npos.X && m_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 (!(m_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(m_Vertexes[curr.id].p, npos, m_EdgesLeft) && CheckVisibilityRight(m_Vertexes[curr.id].p, npos, m_EdgesRight) && CheckVisibilityBottom(m_Vertexes[curr.id].p, npos, m_EdgesBottom) && CheckVisibilityTop(m_Vertexes[curr.id].p, npos, m_EdgesTop) && CheckVisibility(m_Vertexes[curr.id].p, npos, m_EdgesUnaligned); // Render the edges that we examine. DebugRenderEdges(cmpObstructionManager->GetSimContext(), visible, m_Vertexes[curr.id].p, npos); if (visible) { fixed g = m_Vertexes[curr.id].g + (m_Vertexes[curr.id].p - npos).Length(); // If this is a new tile, compute the heuristic distance if (m_Vertexes[n].status == Vertex::UNEXPLORED) { // Add it to the open list: m_Vertexes[n].status = Vertex::OPEN; m_Vertexes[n].g = g; m_Vertexes[n].h = request.goal.DistanceToPoint(npos); m_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 (m_Vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8))) m_Vertexes[n].quadOutward = ((m_Vertexes[n].quadInward) & quad) & 0xF; if (n == GOAL_VERTEX_ID) m_Vertexes[n].p = npos; // remember the new best goal position VertexPriorityQueue::Item t = { (u16)n, g + m_Vertexes[n].h, m_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 (m_Vertexes[n].h < hBest) { idBest = (u16)n; hBest = m_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 >= m_Vertexes[n].g) continue; // Otherwise, we have a better path, so replace the old one with the new cost/parent fixed gprev = m_Vertexes[n].g; m_Vertexes[n].g = g; m_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 (m_Vertexes[n].quadInward) m_Vertexes[n].quadOutward = ((m_Vertexes[n].quadInward) & quad) & 0xF; if (n == GOAL_VERTEX_ID) m_Vertexes[n].p = npos; // remember the new best goal position open.promote((u16)n, gprev + m_Vertexes[n].h, g + m_Vertexes[n].h, m_Vertexes[n].h); } } } } // Reconstruct the path (in reverse) WaypointPath path; for (u16 id = idBest; id != START_VERTEX_ID; id = m_Vertexes[id].pred) path.m_Waypoints.emplace_back(Waypoint{ m_Vertexes[id].p.X, m_Vertexes[id].p.Y }); m_Edges.clear(); m_EdgeSquares.clear(); m_Vertexes.clear(); m_EdgesUnaligned.clear(); m_EdgesLeft.clear(); m_EdgesRight.clear(); m_EdgesBottom.clear(); m_EdgesTop.clear(); return path; } void VertexPathfinder::DebugRenderGoal(const CSimContext& simContext, const PathGoal& goal) const { if (!m_DebugOverlay) return; m_DebugOverlayShortPathLines.clear(); // Render the goal shape m_DebugOverlayShortPathLines.push_back(SOverlayLine()); m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1); switch (goal.type) { case PathGoal::POINT: { SimRender::ConstructCircleOnGround(simContext, goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true); break; } case PathGoal::CIRCLE: case PathGoal::INVERTED_CIRCLE: { SimRender::ConstructCircleOnGround(simContext, goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true); break; } case PathGoal::SQUARE: case PathGoal::INVERTED_SQUARE: { float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat()); SimRender::ConstructSquareOnGround(simContext, goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true); break; } } } void VertexPathfinder::DebugRenderGraph(const CSimContext& simContext, const std::vector& vertexes, const std::vector& edges, const std::vector& edgeSquares) const { if (!m_DebugOverlay) return; #define PUSH_POINT(p) STMT(xz.push_back(p.X.ToFloat()); xz.push_back(p.Y.ToFloat())) // Render the vertexes as little Pac-Man shapes to indicate quadrant direction for (size_t i = 0; i < vertexes.size(); ++i) { m_DebugOverlayShortPathLines.emplace_back(); m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1); float x = vertexes[i].p.X.ToFloat(); float z = vertexes[i].p.Y.ToFloat(); float a0 = 0, a1 = 0; // Get arc start/end angles depending on quadrant (if any) if (vertexes[i].quadInward == QUADRANT_BL) { a0 = -0.25f; a1 = 0.50f; } else if (vertexes[i].quadInward == QUADRANT_TR) { a0 = 0.25f; a1 = 1.00f; } else if (vertexes[i].quadInward == QUADRANT_TL) { a0 = -0.50f; a1 = 0.25f; } else if (vertexes[i].quadInward == QUADRANT_BR) { a0 = 0.00f; a1 = 0.75f; } if (a0 == a1) SimRender::ConstructCircleOnGround(simContext, x, z, 0.5f, m_DebugOverlayShortPathLines.back(), true); else SimRender::ConstructClosedArcOnGround(simContext, x, z, 0.5f, a0 * ((float)M_PI*2.0f), a1 * ((float)M_PI*2.0f), m_DebugOverlayShortPathLines.back(), true); } // Render the edges for (size_t i = 0; i < edges.size(); ++i) { m_DebugOverlayShortPathLines.emplace_back(); m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); std::vector xz; PUSH_POINT(edges[i].p0); PUSH_POINT(edges[i].p1); // Add an arrowhead to indicate the direction CFixedVector2D d = edges[i].p1 - edges[i].p0; d.Normalize(fixed::FromInt(1)/8); CFixedVector2D p2 = edges[i].p1 - d*2; CFixedVector2D p3 = p2 + d.Perpendicular(); CFixedVector2D p4 = p2 - d.Perpendicular(); PUSH_POINT(p3); PUSH_POINT(p4); PUSH_POINT(edges[i].p1); SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), true); } #undef PUSH_POINT // Render the axis-aligned squares for (size_t i = 0; i < edgeSquares.size(); ++i) { m_DebugOverlayShortPathLines.push_back(SOverlayLine()); m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); std::vector xz; Square s = edgeSquares[i]; xz.push_back(s.p0.X.ToFloat()); xz.push_back(s.p0.Y.ToFloat()); xz.push_back(s.p0.X.ToFloat()); xz.push_back(s.p1.Y.ToFloat()); xz.push_back(s.p1.X.ToFloat()); xz.push_back(s.p1.Y.ToFloat()); xz.push_back(s.p1.X.ToFloat()); xz.push_back(s.p0.Y.ToFloat()); xz.push_back(s.p0.X.ToFloat()); xz.push_back(s.p0.Y.ToFloat()); SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), true); } } void VertexPathfinder::DebugRenderEdges(const CSimContext& UNUSED(simContext), bool UNUSED(visible), CFixedVector2D UNUSED(curr), CFixedVector2D UNUSED(npos)) const { if (!m_DebugOverlay) return; // Disabled by default. /* m_DebugOverlayShortPathLines.push_back(SOverlayLine()); m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); 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 xz; xz.push_back(curr.X.ToFloat()); xz.push_back(curr.Y.ToFloat()); xz.push_back(npos.X.ToFloat()); xz.push_back(npos.Y.ToFloat()); SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false); SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false); */ } void VertexPathfinder::RenderSubmit(SceneCollector& collector) { if (!m_DebugOverlay) return; for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i) collector.Submit(&m_DebugOverlayShortPathLines[i]); } Index: ps/trunk/source/simulation2/helpers/VertexPathfinder.h =================================================================== --- ps/trunk/source/simulation2/helpers/VertexPathfinder.h (revision 22304) +++ ps/trunk/source/simulation2/helpers/VertexPathfinder.h (revision 22305) @@ -1,120 +1,120 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #ifndef INCLUDED_VERTEXPATHFINDER #define INCLUDED_VERTEXPATHFINDER #include "graphics/Overlay.h" #include "simulation2/helpers/Pathfinding.h" #include "simulation2/system/CmpPtr.h" // A vertex around the corners of an obstruction // (paths will be sequences of these vertexes) struct Vertex { enum { UNEXPLORED, OPEN, CLOSED, }; CFixedVector2D p; fixed g, h; u16 pred; u8 status; u8 quadInward : 4; // the quadrant which is inside the shape (or NONE) u8 quadOutward : 4; // the quadrants of the next point on the path which this vertex must be in, given 'pred' }; // Obstruction edges (paths will not cross any of these). // Defines the two points of the edge. struct Edge { CFixedVector2D p0, p1; }; // Axis-aligned obstruction squares (paths will not cross any of these). // Defines the opposing corners of an axis-aligned square // (from which four individual edges can be trivially computed), requiring p0 <= p1 struct Square { CFixedVector2D p0, p1; }; // Axis-aligned obstruction edges. // p0 defines one end; c1 is either the X or Y coordinate of the other end, // depending on the context in which this is used. struct EdgeAA { CFixedVector2D p0; fixed c1; }; class ICmpObstructionManager; class CSimContext; class SceneCollector; class VertexPathfinder { public: VertexPathfinder(const u16& mapSize, Grid* const & terrainOnlyGrid) : m_MapSize(mapSize), m_TerrainOnlyGrid(terrainOnlyGrid), m_DebugOverlay(false) {}; /** * Compute a precise path from the given point to the goal, and return the set of waypoints. * The path is based on the full set of obstructions that pass the filter, such that * a unit of clearance 'clearance' will be able to follow the path with no collisions. * The path is restricted to a box of radius 'range' from the starting point. * Defined in CCmpPathfinder_Vertex.cpp */ - WaypointPath ComputeShortPath(const AsyncShortPathRequest& request, CmpPtr cmpObstructionManager) const; + WaypointPath ComputeShortPath(const ShortPathRequest& request, CmpPtr cmpObstructionManager) const; void SetDebugOverlay(bool enabled) { m_DebugOverlay = enabled; } void RenderSubmit(SceneCollector& collector); private: void DebugRenderGoal(const CSimContext& simContext, const PathGoal& goal) const; void DebugRenderGraph(const CSimContext& simContext, const std::vector& vertexes, const std::vector& edges, const std::vector& edgeSquares) const; void DebugRenderEdges(const CSimContext& simContext, bool visible, CFixedVector2D curr, CFixedVector2D npos) const; // References to the Pathfinder for convenience. const u16& m_MapSize; Grid* const & m_TerrainOnlyGrid; std::atomic m_DebugOverlay; mutable std::vector m_DebugOverlayShortPathLines; // These vectors are expensive to recreate on every call, so we cache them here. // They are made mutable to allow using them in the otherwise const ComputeShortPath. mutable std::vector m_EdgesUnaligned; mutable std::vector m_EdgesLeft; mutable std::vector m_EdgesRight; mutable std::vector m_EdgesBottom; mutable std::vector m_EdgesTop; // List of obstruction vertexes (plus start/end points); we'll try to find paths through // the graph defined by these vertexes. mutable std::vector m_Vertexes; // 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.) mutable std::vector m_Edges; mutable std::vector m_EdgeSquares; // Axis-aligned squares; equivalent to 4 edges. }; #endif // INCLUDED_VERTEXPATHFINDER