Index: binaries/data/mods/public/gui/options/options.json =================================================================== --- binaries/data/mods/public/gui/options/options.json +++ binaries/data/mods/public/gui/options/options.json @@ -81,7 +81,22 @@ "label": "Chat Timestamp", "tooltip": "Show time that messages are posted in the lobby, gamesetup and ingame chat.", "config": "chat.timestamp" + }, + { + "type": "boolean", + "label": "Use non-standerd pathfinder thread count", + "config": "pathfinder.usenumthreads" + }, + { + "type": "number", + "label": "Number of pathfinder threads", + "tooltip": "Number of pathfinder worker threads. Mostly for debugging purposes. 0 to disable threading.", + "config": "pathfinder.numthreads", + "dependencies":["pathfinder.usenumthreads"], + "min": 0, + "max": 64 } + ] }, { @@ -378,10 +393,10 @@ "options": [ { - "type": "boolean", - "label": "TLS Encryption", - "tooltip": "Protect login and data exchanged with the lobby server using TLS encryption.", - "config": "lobby.tls" + "type": "boolean", + "label": "TLS Encryption", + "tooltip": "Protect login and data exchanged with the lobby server using TLS encryption.", + "config": "lobby.tls" }, { "type": "number", Index: binaries/data/mods/public/simulation/data/pathfinder.xml =================================================================== --- binaries/data/mods/public/simulation/data/pathfinder.xml +++ binaries/data/mods/public/simulation/data/pathfinder.xml @@ -1,8 +1,7 @@ - - 64 + 6 Index: source/ps/Profile.h =================================================================== --- source/ps/Profile.h +++ source/ps/Profile.h @@ -178,15 +178,17 @@ if (CProfileManager::IsInitialised()) { // The profiler is only safe to use on the main thread - ENSURE(ThreadUtil::IsMainThread()); + //ENSURE(ThreadUtil::IsMainThread()); - g_Profiler.Start(name); + if(ThreadUtil::IsMainThread()) + g_Profiler.Start(name); } } ~CProfileSample() { if (CProfileManager::IsInitialised()) - g_Profiler.Stop(); + if(ThreadUtil::IsMainThread()) + g_Profiler.Stop(); } }; Index: source/simulation2/Simulation2.cpp =================================================================== --- source/simulation2/Simulation2.cpp +++ source/simulation2/Simulation2.cpp @@ -540,8 +540,8 @@ CmpPtr cmpPathfinder(simContext, SYSTEM_ENTITY); if (cmpPathfinder) { + cmpPathfinder->FetchAsyncResultsAndSendMessages(); cmpPathfinder->UpdateGrid(); - cmpPathfinder->FinishAsyncRequests(); } // Push AI commands onto the queue before we use them @@ -554,24 +554,25 @@ cmpCommandQueue->FlushTurn(commands); // Process newly generated move commands so the UI feels snappy + // This can safely take place during Update since nothing will change the obstruction manager + // TODO: verify this. if (cmpPathfinder) - cmpPathfinder->ProcessSameTurnMoves(); - + { + cmpPathfinder->StartProcessingMoves(true); + cmpPathfinder->FetchAsyncResultsAndSendMessages(); + } // Send all the update phases { PROFILE2("Sim - Update"); CMessageUpdate msgUpdate(turnLengthFixed); componentManager.BroadcastMessage(msgUpdate); } + { CMessageUpdate_MotionFormation msgUpdate(turnLengthFixed); componentManager.BroadcastMessage(msgUpdate); } - // Process move commands for formations (group proxy) - if (cmpPathfinder) - cmpPathfinder->ProcessSameTurnMoves(); - { PROFILE2("Sim - Motion Unit"); CMessageUpdate_MotionUnit msgUpdate(turnLengthFixed); @@ -583,12 +584,12 @@ componentManager.BroadcastMessage(msgUpdate); } - // Process moves resulting from group proxy movement (unit needs to catch up or realign) and any others - if (cmpPathfinder) - cmpPathfinder->ProcessSameTurnMoves(); - // Clean up any entities destroyed during the simulation update componentManager.FlushDestroyedComponents(); + + // Process all remaining moves + if (cmpPathfinder) + cmpPathfinder->StartProcessingMoves(); } void CSimulation2Impl::Interpolate(float simFrameLength, float frameOffset, float realFrameLength) Index: source/simulation2/components/CCmpOverlayRenderer.cpp =================================================================== --- source/simulation2/components/CCmpOverlayRenderer.cpp +++ source/simulation2/components/CCmpOverlayRenderer.cpp @@ -80,14 +80,14 @@ { case MT_Interpolate: { - PROFILE("OverlayRenderer::Interpolate"); + //PROFILE("OverlayRenderer::Interpolate"); const CMessageInterpolate& msgData = static_cast (msg); Interpolate(msgData.deltaSimTime, msgData.offset); break; } case MT_RenderSubmit: { - PROFILE("OverlayRenderer::RenderSubmit"); + //PROFILE("OverlayRenderer::RenderSubmit"); const CMessageRenderSubmit& msgData = static_cast (msg); RenderSubmit(msgData.collector); break; Index: source/simulation2/components/CCmpPathfinder.cpp =================================================================== --- source/simulation2/components/CCmpPathfinder.cpp +++ source/simulation2/components/CCmpPathfinder.cpp @@ -28,6 +28,7 @@ #include "ps/CStr.h" #include "ps/Profile.h" #include "ps/XML/Xeromyces.h" +#include "ps/ConfigDB.h" #include "renderer/Scene.h" #include "simulation2/MessageTypes.h" #include "simulation2/components/ICmpObstruction.h" @@ -37,215 +38,247 @@ #include "simulation2/helpers/Rasterize.h" #include "simulation2/serialization/SerializeTemplates.h" +#include "tools/atlas/GameInterface/GameLoop.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_DebugOverlay = false; - m_AtlasOverlay = NULL; - - m_SameTurnMovesCount = 0; - - // 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; - } + m_MapSize = 0; + m_Grid = NULL; + m_TerrainOnlyGrid = NULL; + + FlushAIPathfinderDirtinessInformation(); + + m_NextAsyncTicket = 1; + + m_DebugOverlay = false; + m_AtlasOverlay = NULL; + + // 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"); + + 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; + } + + // check if user wants to use custom thread count. + + bool useCustomThreadCount = false; + CFG_GET_VAL("pathfinder.usenumthreads", useCustomThreadCount); + if (useCustomThreadCount) + { + int threadcount; + CFG_GET_VAL("pathfinder.numthreads", threadcount); + + std::cerr << "Using " << threadcount << "pathfinder threads" << "\n"; + + if (threadcount < 1 || (g_AtlasGameLoop && g_AtlasGameLoop->running) || threadcount > 64) + { + m_UseThreading = false; + m_WorkerThreads.push_back(new AsyncPathfinderWorkerThread(*this)); + } + else + { + for (size_t i = 0; i < threadcount; ++i) + m_WorkerThreads.push_back(new AsyncPathfinderWorkerThread(*this)); + } + } + else + { + // do not use threads if we are single-core (obviously) or are in atlas (as we might modify state in-between turns). + // the worker thread will only call std::thread if we're using threading, so if not it'll be synchronous and single-thread. + if (std::thread::hardware_concurrency() <= 1 || (g_AtlasGameLoop && g_AtlasGameLoop->running)) + { + m_UseThreading = false; + m_WorkerThreads.push_back(new AsyncPathfinderWorkerThread(*this)); + } + else + { + // create one thread if we have two cores, otherwise 2*(number of cores-1) + // this way we'll benefit from hardware load-balancing a little. + if (std::thread::hardware_concurrency() == 2) + m_WorkerThreads.push_back(new AsyncPathfinderWorkerThread(*this)); + else + for (size_t i = 0; i < (std::thread::hardware_concurrency() - 1) * 2; ++i) + m_WorkerThreads.push_back(new AsyncPathfinderWorkerThread(*this)); + } + } } void CCmpPathfinder::Deinit() { - SetDebugOverlay(false); // cleans up memory - SAFE_DELETE(m_AtlasOverlay); + for (auto worker : m_WorkerThreads) + worker->PrepareToKill(); + m_MainThreadSignal.notify_all(); + for (auto worker : m_WorkerThreads) + delete worker; + + m_WorkerThreads.clear(); + SetDebugOverlay(false); // cleans up memory + SAFE_DELETE(m_AtlasOverlay); - SAFE_DELETE(m_Grid); - SAFE_DELETE(m_TerrainOnlyGrid); + SAFE_DELETE(m_Grid); + SAFE_DELETE(m_TerrainOnlyGrid); } struct SerializeLongRequest { - template - void operator()(S& serialize, const char* UNUSED(name), AsyncLongPathRequest& 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); - } + template + void operator()(S& serialize, const char* UNUSED(name), AsyncLongPathRequest& 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) - { - 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 operator()(S& serialize, const char* UNUSED(name), AsyncShortPathRequest& 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); - serialize.NumberU32_Unbounded("next ticket", m_NextAsyncTicket); - serialize.NumberU16_Unbounded("same turn moves count", m_SameTurnMovesCount); - serialize.NumberU16_Unbounded("map size", m_MapSize); + SerializeVector()(serialize, "long requests", m_AsyncLongPathRequests); + SerializeVector()(serialize, "short requests", m_AsyncShortPathRequests); + serialize.NumberU32_Unbounded("next ticket", m_NextAsyncTicket); + serialize.NumberU16_Unbounded("map size", m_MapSize); } void CCmpPathfinder::Serialize(ISerializer& serialize) { - SerializeCommon(serialize); + SerializeCommon(serialize); } void CCmpPathfinder::Deserialize(const CParamNode& paramNode, IDeserializer& deserialize) { - Init(paramNode); + Init(paramNode); - SerializeCommon(deserialize); + 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; - } + 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; + } } void CCmpPathfinder::RenderSubmit(SceneCollector& collector) { - for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i) - collector.Submit(&m_DebugOverlayShortPathLines[i]); + for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i) + collector.Submit(&m_DebugOverlayShortPathLines[i]); - m_LongPathfinder.HierarchicalRenderSubmit(collector); + m_LongPathfinder.HierarchicalRenderSubmit(collector); } 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); + 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; - } + 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; + return it->second; } void CCmpPathfinder::GetPassabilityClasses(std::map& passClasses) const { - passClasses = m_PassClassMasks; + passClasses = m_PassClassMasks; } -void CCmpPathfinder::GetPassabilityClasses(std::map& nonPathfindingPassClasses, std::map& pathfindingPassClasses) const +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; - } + 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; - } + for (const PathfinderPassability& passability : m_PassClasses) + { + if (passability.m_Mask == passClass) + return &passability; + } - return NULL; + return NULL; } const Grid& CCmpPathfinder::GetPassabilityGrid() { - if (!m_Grid) - UpdateGrid(); + if (!m_Grid) + UpdateGrid(); - return *m_Grid; + return *m_Grid; } /** @@ -262,629 +295,796 @@ */ static void ExpandImpassableCells(Grid& grid, u16 clearance, pass_class_t mask) { - PROFILE3("ExpandImpassableCells"); + //PROFILE3("ExpandImpassableCells"); - u16 w = grid.m_W; - u16 h = grid.m_H; + 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; - } - } + // 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"); + //PROFILE3("ComputeShoreGrid"); - CmpPtr cmpWaterManager(GetSystemEntity()); + CmpPtr cmpWaterManager(GetSystemEntity()); - // TODO: these bits should come from ICmpTerrain - CTerrain& terrain = GetSimContext().GetTerrain(); + // 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; - } - } - } + // 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; + 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); + //PROFILE3("UpdateGrid"); - m_DirtinessInformation = { true, true, Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE) }; - m_AIPathfinderDirtinessInformation = m_DirtinessInformation; + 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; - } + m_TerrainDirty = true; + } - // The grid should be properly initialized and clean. Checking the latter is expensive so do it only for debugging. + // 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)); + 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)); + 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); + CmpPtr cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY); + cmpObstructionManager->UpdateInformations(m_DirtinessInformation); - if (!m_DirtinessInformation.dirty && !m_TerrainDirty) - return; + 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 pathfinder - if (m_DirtinessInformation.globallyDirty) - { - std::map nonPathfindingPassClasses, pathfindingPassClasses; - GetPassabilityClasses(nonPathfindingPassClasses, pathfindingPassClasses); - m_LongPathfinder.Reload(m_Grid, nonPathfindingPassClasses, pathfindingPassClasses); - } - else - m_LongPathfinder.Update(m_Grid, m_DirtinessInformation.dirtinessGrid); + // 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 pathfinder + if (m_DirtinessInformation.globallyDirty) + { + std::map nonPathfindingPassClasses, pathfindingPassClasses; + GetPassabilityClasses(nonPathfindingPassClasses, pathfindingPassClasses); + m_LongPathfinder.Reload(m_Grid, nonPathfindingPassClasses, pathfindingPassClasses); + } + else + m_LongPathfinder.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); + // Remember the necessary updates that the AI pathfinder will have to perform as well + m_AIPathfinderDirtinessInformation.MergeAndClear(m_DirtinessInformation); } void CCmpPathfinder::MinimalTerrainUpdate() { - TerrainUpdateHelper(false); + TerrainUpdateHelper(false); } void CCmpPathfinder::TerrainUpdateHelper(bool expandPassability/* = true */) { - PROFILE3("TerrainUpdateHelper"); + //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); - } + 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); + } } ////////////////////////////////////////////////////////// -// Async path requests: +// Async pathfinder workers -u32 CCmpPathfinder::ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) +CCmpPathfinder::AsyncPathfinderWorkerThread::AsyncPathfinderWorkerThread(const CCmpPathfinder& pathfinder) + : m_Pathfinder(pathfinder) { - AsyncLongPathRequest req = { m_NextAsyncTicket++, x0, z0, goal, passClass, notify }; - m_AsyncLongPathRequests.push_back(req); - return req.ticket; + if (m_Pathfinder.m_UseThreading) + m_Thread = new std::thread(&CCmpPathfinder::AsyncPathfinderWorkerThread::InitThread, this, + m_Pathfinder.m_WorkerThreads.size()); } -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) +CCmpPathfinder::AsyncPathfinderWorkerThread::~AsyncPathfinderWorkerThread() { - AsyncShortPathRequest req = { m_NextAsyncTicket++, x0, z0, clearance, range, goal, passClass, avoidMovingUnits, group, notify }; - m_AsyncShortPathRequests.push_back(req); - return req.ticket; + if (m_Thread != nullptr) + { + if (m_Thread->joinable()) + m_Thread->join(); + delete m_Thread; + } } -void CCmpPathfinder::FinishAsyncRequests() +void CCmpPathfinder::AsyncPathfinderWorkerThread::InitThread(size_t index) { - PROFILE2("Finish Async Requests"); - // Save the request queue in case it gets modified while iterating - std::vector longRequests; - m_AsyncLongPathRequests.swap(longRequests); - - std::vector shortRequests; - m_AsyncShortPathRequests.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); + g_Profiler2.RegisterCurrentThread("Pathfinder thread " + std::to_string(index)); + WaitForWork(); } -void CCmpPathfinder::ProcessLongRequests(const std::vector& longRequests) +void CCmpPathfinder::AsyncPathfinderWorkerThread::PrepareToKill() { - PROFILE2("Process Long Requests"); - for (size_t i = 0; i < longRequests.size(); ++i) - { - const AsyncLongPathRequest& 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); - } + m_Kill = true; } -void CCmpPathfinder::ProcessShortRequests(const std::vector& shortRequests) +void CCmpPathfinder::AsyncPathfinderWorkerThread::WaitForWork() { - PROFILE2("Process Short Requests"); - for (size_t i = 0; i < shortRequests.size(); ++i) - { - const AsyncShortPathRequest& req = shortRequests[i]; - WaypointPath path; - ControlGroupMovementObstructionFilter filter(req.avoidMovingUnits, req.group); - ComputeShortPath(filter, req.x0, req.z0, req.clearance, req.range, req.goal, req.passClass, path); - CMessagePathResult msg(req.ticket, path); - GetSimContext().GetComponentManager().PostMessage(req.notify, msg); - } + while (!m_Kill) + { + // wait until we're woken up + std::unique_lock lock(m_Pathfinder.m_MainThreadMutex); + m_Pathfinder.m_MainThreadSignal.wait(lock); + lock.unlock(); + + Work(); + } } -void CCmpPathfinder::ProcessSameTurnMoves() +void CCmpPathfinder::AsyncPathfinderWorkerThread::Work() { - if (!m_AsyncLongPathRequests.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) - { - m_AsyncLongPathRequests.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); - } + m_Computing = true; - ProcessLongRequests(longRequests); + // start by going through our long requests. + while (!m_LongRequests.empty()) + { + std::unique_lock lock(m_Pause); + if (!m_Pathfinder.m_MayComputePaths) + return; - m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); - } + const AsyncLongPathRequest& req = m_LongRequests.back(); + WaypointPath path; + ComputePath(req.x0, req.z0, req.goal, req.passClass, path); + m_Results.push_back({req.ticket, req.notify, path}); - if (!m_AsyncShortPathRequests.empty()) - { - // Figure out how many moves we can do now - i32 moveCount = m_MaxSameTurnMoves - m_SameTurnMovesCount; + m_LongRequests.pop_back(); + lock.unlock(); + } + // then the short requests + while (!m_ShortRequests.empty()) + { + std::unique_lock lock(m_Pause); + if (!m_Pathfinder.m_MayComputePaths) + return; - if (moveCount <= 0) - return; + const AsyncShortPathRequest& req = m_ShortRequests.back(); + WaypointPath path; + ControlGroupMovementObstructionFilter filter(req.avoidMovingUnits, req.group); + ComputeShortPath(filter, req.x0, req.z0, req.clearance, req.range, req.goal, req.passClass, path); + m_Results.push_back({req.ticket, req.notify, path}); - // Copy the short request elements we are going to process into a new array - std::vector shortRequests; - if ((i32)m_AsyncShortPathRequests.size() <= moveCount) - { - m_AsyncShortPathRequests.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); - } + m_ShortRequests.pop_back(); + lock.unlock(); + } + std::unique_lock lock(m_Pause); + m_Computing = false; + lock.unlock(); +} - ProcessShortRequests(shortRequests); +// Async path requests: - m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); - } +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); + return req.ticket; +} + +void +CCmpPathfinder::ComputePathImmediate(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, + WaypointPath& ret) const +{ + m_LongPathfinder.ComputePath(x0, z0, goal, passClass, ret); +} + +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); + return req.ticket; +} + +void CCmpPathfinder::FetchAsyncResultsAndSendMessages() +{ + // wait until all threads have stopped computing + // collect messages + // send them + //PROFILE2("FetchAsyncResults"); + + + + while (true) + { + bool stop = true; + + + if (stop) + break; + } + + while (true) + { + bool stop = true; + for (auto worker : m_WorkerThreads) + if (worker->m_Computing) + stop = false; + if (stop) + break; + } + + std::vector results; + for (auto worker : m_WorkerThreads) + { + results.insert(results.end(), std::make_move_iterator(worker->m_Results.begin()), + std::make_move_iterator(worker->m_Results.end())); + worker->m_Results.clear(); + } + // post messages + { + //PROFILE2("PostMessages"); + for (PathResult& path : results) + { + CMessagePathResult msg(path.ticket, path.path); + GetSimContext().GetComponentManager().PostMessage(path.notify, msg); + } + } +} + +void CCmpPathfinder::StartProcessingMoves(bool useMax) +{ + // We will send new path requests to worker threads + // trying to balance the workload + // and then notify them they can start working. + // since it's entirely possible that they are still currently computing paths + // we will lock on the m_Pause mutex of each thread to pause them temporarily + + size_t done = 0; + + std::vector longRequests; + if (useMax) + { + size_t amount = std::min(m_AsyncLongPathRequests.size(), (size_t) m_MaxSameTurnMoves); + if (amount > 0) + { + longRequests.insert(longRequests.begin(), + std::make_move_iterator(m_AsyncLongPathRequests.end() - amount), + std::make_move_iterator(m_AsyncLongPathRequests.end())); + m_AsyncLongPathRequests.erase(m_AsyncLongPathRequests.end() - amount, m_AsyncLongPathRequests.end()); + } + } + else + { + longRequests.swap(m_AsyncLongPathRequests); + m_AsyncLongPathRequests.clear(); + } + + done += longRequests.size(); + + if (!longRequests.empty()) + { + size_t amount = longRequests.size() / m_WorkerThreads.size(); + // try and divide equally among threads. + for (size_t i = 0; i < m_WorkerThreads.size() - 1; ++i) + { + std::unique_lock lock(m_WorkerThreads[i]->m_Pause); + // move-insert + m_WorkerThreads[i]->m_LongRequests.insert(m_WorkerThreads[i]->m_LongRequests.begin(), + std::make_move_iterator(longRequests.end() - amount), + std::make_move_iterator(longRequests.end())); + lock.unlock(); + longRequests.erase(longRequests.end() - amount, longRequests.end()); + } + // move the whole thing for the last, to make sure we don't forget one through rounding. + std::unique_lock lock(m_WorkerThreads.back()->m_Pause); + m_WorkerThreads.back()->m_LongRequests.insert(m_WorkerThreads.back()->m_LongRequests.begin(), + std::make_move_iterator(longRequests.begin()), + std::make_move_iterator(longRequests.end())); + lock.unlock(); + } + + std::vector shortRequests; + if (useMax) + { + size_t amount = std::min(m_AsyncShortPathRequests.size(), (size_t) m_MaxSameTurnMoves - done); + if (amount > 0) + { + shortRequests.insert(shortRequests.begin(), + std::make_move_iterator(m_AsyncShortPathRequests.end() - amount), + std::make_move_iterator(m_AsyncShortPathRequests.end())); + m_AsyncShortPathRequests.erase(m_AsyncShortPathRequests.end() - amount, m_AsyncShortPathRequests.end()); + } + } + else + { + shortRequests.swap(m_AsyncShortPathRequests); + m_AsyncShortPathRequests.clear(); + } + + if (!shortRequests.empty()) + { + size_t amount = shortRequests.size() / m_WorkerThreads.size(); + // try and divide equally among threads. + for (size_t i = 0; i < m_WorkerThreads.size() - 1; ++i) + { + std::unique_lock lock(m_WorkerThreads[i]->m_Pause); + // move-insert + m_WorkerThreads[i]->m_ShortRequests.insert(m_WorkerThreads[i]->m_ShortRequests.begin(), + std::make_move_iterator(shortRequests.end() - amount), + std::make_move_iterator(shortRequests.end())); + lock.unlock(); + shortRequests.erase(shortRequests.end() - amount, shortRequests.end()); + } + // move the whole thing for the last, to make sure we don't forget one through rounding. + std::unique_lock lock(m_WorkerThreads.back()->m_Pause); + m_WorkerThreads.back()->m_ShortRequests.insert(m_WorkerThreads.back()->m_ShortRequests.begin(), + std::make_move_iterator(shortRequests.begin()), + std::make_move_iterator(shortRequests.end())); + lock.unlock(); + } + + // we may now notify them. + m_MayComputePaths = true; + if (m_UseThreading) + { + // immediately mark them as computing + for (auto worker : m_WorkerThreads) + { + std::unique_lock lock(worker->m_Pause); + worker->m_Computing = true; + lock.unlock(); + } + std::lock_guard lock(m_MainThreadMutex); + m_MainThreadSignal.notify_all(); + } + else + m_WorkerThreads.back()->Work(); } ////////////////////////////////////////////////////////// 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 + 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); + //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); + // 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; + 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) - // 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; + 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 + 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); + 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; - } + 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; + return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; } Index: source/simulation2/components/CCmpPathfinder_Common.h =================================================================== --- source/simulation2/components/CCmpPathfinder_Common.h +++ source/simulation2/components/CCmpPathfinder_Common.h @@ -38,6 +38,10 @@ #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/helpers/LongPathfinder.h" +#include +#include +#include + class SceneCollector; class AtlasOverlay; @@ -47,7 +51,6 @@ #define PATHFIND_DEBUG 1 #endif - struct AsyncLongPathRequest { u32 ticket; @@ -120,6 +123,91 @@ */ class CCmpPathfinder : public ICmpPathfinder { +protected: + + class AsyncPathfinderWorkerThread + { + friend CCmpPathfinder; + private: + // takes care of what needs to be called to initialise the thread before calling WaitForWork(); + void InitThread(size_t index); + public: + AsyncPathfinderWorkerThread(const CCmpPathfinder& pathfinder); + ~AsyncPathfinderWorkerThread(); + + void PrepareToKill(); + + // Will loop until a conditional_variable notifies us, and call Work(). + void WaitForWork(); + // Process path requests, checking if we should stop before each new one. Sets m_Computing to false on return. + void Work(); + + /** + * 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. + */ + void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) + { + m_Pathfinder.m_LongPathfinder.ComputePath(x0, z0, goal, passClass, ret); + } + + /** + * 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 + */ + void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret); + + private: + /* + * General state + */ + + // read-only pathfinder, for getting required information (grids,…) + const CCmpPathfinder& m_Pathfinder; + + // store our results, the main thread will fetch this + std::vector m_Results; + + std::thread* m_Thread = nullptr; + + bool m_Kill = false; + + /* + * Thread synchronisation + */ + + bool m_Computing = false; + // using deque: for simplicity the worker thread will pop_back, the main thread will push_front + std::deque m_LongRequests; + std::deque m_ShortRequests; + // the main thread may push new requests even when we are computing existing ones, so lock. + std::mutex m_Pause; + + /* + * Memory optimisations: avoid recreating these vectors + */ + + std::vector edgesUnaligned; + std::vector edgesLeft; + std::vector edgesRight; + std::vector edgesBottom; + std::vector edgesTop; + + // List of obstruction vertexes (plus start/end points); we'll try to find paths through + // the graph defined by these vertexes + std::vector 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.) + std::vector edges; + std::vector edgeSquares; // axis-aligned squares; equivalent to 4 edges}; + }; +// Allow the workers to access our private variables +friend class AsyncPathfinderWorkerThread; + public: static void ClassInit(CComponentManager& componentManager) { @@ -143,7 +231,7 @@ std::vector m_AsyncLongPathRequests; std::vector m_AsyncShortPathRequests; u32 m_NextAsyncTicket; // unique IDs for asynchronous path requests - u16 m_SameTurnMovesCount; // current number of same turn moves we have processed this turn + u16 m_MaxSameTurnMoves; // How many moves to immediately compute. // Lazily-constructed dynamic state (not serialized): @@ -159,29 +247,25 @@ bool m_TerrainDirty; // Interface to the long-range pathfinder. - LongPathfinder m_LongPathfinder; + mutable LongPathfinder 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 - - // memory optimizations: those vectors are created once, reused for all calculations; - std::vector edgesUnaligned; - std::vector edgesLeft; - std::vector edgesRight; - std::vector edgesBottom; - std::vector edgesTop; - - // List of obstruction vertexes (plus start/end points); we'll try to find paths through - // the graph defined by these vertexes - std::vector 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.) - std::vector edges; - std::vector edgeSquares; // axis-aligned squares; equivalent to 4 edges + // we Process paths in an asynchronous way, making sure to do it at times where it is safe. + std::vector m_WorkerThreads; + bool m_UseThreading = true; + + // this variable will be set by the main thread. When going from true to false, + // the threads will define their own m_Computing to false. The main thread must wait until + // all threads' m_Computing are false to proceed. + bool m_MayComputePaths = false; + + // we use a condition variable to wake the pathfinder threads when relevant + mutable std::condition_variable m_MainThreadSignal; + mutable std::mutex m_MainThreadMutex; + // HACK: the pathfinder is threaded but we will want to debug, so use mutexes and mutable to modify this. bool m_DebugOverlay; - std::vector m_DebugOverlayShortPathLines; + mutable std::vector m_DebugOverlayShortPathLines; + mutable std::mutex m_DebugMutex; AtlasOverlay* m_AtlasOverlay; static std::string GetSchema() @@ -245,17 +329,12 @@ 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) - { - m_LongPathfinder.ComputePath(x0, z0, goal, passClass, ret); - } - virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify); - virtual void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret); - 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 ComputePathImmediate(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) const; + virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) { m_LongPathfinder.SetDebugPath(x0, z0, goal, passClass); @@ -287,13 +366,9 @@ 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 ProcessShortRequests(const std::vector& shortRequests); + virtual void FetchAsyncResultsAndSendMessages(); - virtual void ProcessSameTurnMoves(); + virtual void StartProcessingMoves(bool useMax); /** * Regenerates the grid based on the current obstruction list, if necessary Index: source/simulation2/components/CCmpPathfinder_Vertex.cpp =================================================================== --- source/simulation2/components/CCmpPathfinder_Vertex.cpp +++ source/simulation2/components/CCmpPathfinder_Vertex.cpp @@ -257,7 +257,7 @@ int i0, int j0, int i1, int j1, pass_class_t passClass, const Grid& grid) { - PROFILE("AddTerrainEdges"); + //PROFILE2("AddTerrainEdges"); // 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) @@ -509,39 +509,42 @@ } }; -void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, +void CCmpPathfinder::AsyncPathfinderWorkerThread::ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& path) { - PROFILE("ComputeShortPath"); - m_DebugOverlayShortPathLines.clear(); + //PROFILE2("ComputeShortPath"); - if (m_DebugOverlay) + if (m_Pathfinder.m_DebugOverlay) { + std::unique_lock lock(m_Pathfinder.m_DebugMutex); + m_Pathfinder.m_DebugOverlayShortPathLines.clear(); + // Render the goal shape - m_DebugOverlayShortPathLines.push_back(SOverlayLine()); - m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1); + m_Pathfinder.m_DebugOverlayShortPathLines.push_back(SOverlayLine()); + m_Pathfinder.m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1); switch (goal.type) { case PathGoal::POINT: { - SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true); + SimRender::ConstructCircleOnGround(m_Pathfinder.GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_Pathfinder.m_DebugOverlayShortPathLines.back(), true); break; } case PathGoal::CIRCLE: case PathGoal::INVERTED_CIRCLE: { - SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true); + SimRender::ConstructCircleOnGround(m_Pathfinder.GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_Pathfinder.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(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true); + SimRender::ConstructSquareOnGround(m_Pathfinder.GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_Pathfinder.m_DebugOverlayShortPathLines.back(), true); break; } } + lock.unlock(); } // List of collision edges - paths must never cross these. @@ -578,7 +581,7 @@ const size_t GOAL_VERTEX_ID = 1; // Find all the obstruction squares that might affect us - CmpPtr cmpObstructionManager(GetSystemEntity()); + CmpPtr cmpObstructionManager(m_Pathfinder.GetSystemEntity()); std::vector squares; size_t staticShapesNb = 0; cmpObstructionManager->GetStaticObstructionsInRange(filter, rangeXMin - clearance, rangeZMin - clearance, rangeXMax + clearance, rangeZMax + clearance, squares); @@ -661,9 +664,9 @@ // 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(edges, vertexes, i0, j0, i1, j1, passClass, *m_TerrainOnlyGrid); + Pathfinding::NearestNavcell(rangeXMin, rangeZMin, i0, j0, m_Pathfinder.m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_Pathfinder.m_MapSize*Pathfinding::NAVCELLS_PER_TILE); + Pathfinding::NearestNavcell(rangeXMax, rangeZMax, i1, j1, m_Pathfinder.m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_Pathfinder.m_MapSize*Pathfinding::NAVCELLS_PER_TILE); + AddTerrainEdges(edges, vertexes, i0, j0, i1, j1, passClass, *m_Pathfinder.m_TerrainOnlyGrid); } // Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable) @@ -689,14 +692,15 @@ ENSURE(vertexes.size() < 65536); // we store array indexes as u16 // Render the debug overlay - if (m_DebugOverlay) + if (m_Pathfinder.m_DebugOverlay) { + std::unique_lock lock(m_Pathfinder.m_DebugMutex); #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); + m_Pathfinder.m_DebugOverlayShortPathLines.emplace_back(); + m_Pathfinder.m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1); float x = vertexes[i].p.X.ToFloat(); float z = vertexes[i].p.Y.ToFloat(); @@ -709,19 +713,19 @@ else if (vertexes[i].quadInward == QUADRANT_BR) { a0 = 0.00f; a1 = 0.75f; } if (a0 == a1) - SimRender::ConstructCircleOnGround(GetSimContext(), x, z, 0.5f, - m_DebugOverlayShortPathLines.back(), true); + SimRender::ConstructCircleOnGround(m_Pathfinder.GetSimContext(), x, z, 0.5f, + m_Pathfinder.m_DebugOverlayShortPathLines.back(), true); else - SimRender::ConstructClosedArcOnGround(GetSimContext(), x, z, 0.5f, + SimRender::ConstructClosedArcOnGround(m_Pathfinder.GetSimContext(), x, z, 0.5f, a0 * ((float)M_PI*2.0f), a1 * ((float)M_PI*2.0f), - m_DebugOverlayShortPathLines.back(), true); + m_Pathfinder.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); + m_Pathfinder.m_DebugOverlayShortPathLines.emplace_back(); + m_Pathfinder.m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); std::vector xz; PUSH_POINT(edges[i].p0); PUSH_POINT(edges[i].p1); @@ -736,15 +740,15 @@ PUSH_POINT(p4); PUSH_POINT(edges[i].p1); - SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); + SimRender::ConstructLineOnGround(m_Pathfinder.GetSimContext(), xz, m_Pathfinder.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); + m_Pathfinder.m_DebugOverlayShortPathLines.push_back(SOverlayLine()); + m_Pathfinder.m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); std::vector xz; Square s = edgeSquares[i]; xz.push_back(s.p0.X.ToFloat()); @@ -757,8 +761,9 @@ xz.push_back(s.p0.Y.ToFloat()); xz.push_back(s.p0.X.ToFloat()); xz.push_back(s.p0.Y.ToFloat()); - SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); + SimRender::ConstructLineOnGround(m_Pathfinder.GetSimContext(), xz, m_Pathfinder.m_DebugOverlayShortPathLines.back(), true); } + lock.unlock(); } // Do an A* search over the vertex/visibility graph: @@ -772,7 +777,7 @@ // we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked // as closed), we won't be doing any redundant visibility computations. - PROFILE_START("Short pathfinding - A*"); + //PROFILE_START("Short pathfinding - A*"); VertexPriorityQueue open; VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h }; @@ -853,16 +858,21 @@ CheckVisibility(vertexes[curr.id].p, npos, edgesUnaligned); /* - // Render the edges that we examine - m_DebugOverlayShortPathLines.push_back(SOverlayLine()); - m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); - std::vector xz; - xz.push_back(vertexes[curr.id].p.X.ToFloat()); - xz.push_back(vertexes[curr.id].p.Y.ToFloat()); - xz.push_back(npos.X.ToFloat()); - xz.push_back(npos.Y.ToFloat()); - SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false); - */ + // Render the edges that we examine +- m_DebugOverlayShortPathLines.push_back(SOverlayLine()); +- m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); ++ std::unique_lock lock(m_Pathfinder.m_DebugMutex); ++ m_Pathfinder.m_DebugOverlayShortPathLines.push_back(SOverlayLine()); ++ m_Pathfinder.m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); + std::vector xz; + xz.push_back(vertexes[curr.id].p.X.ToFloat()); + xz.push_back(vertexes[curr.id].p.Y.ToFloat()); + xz.push_back(npos.X.ToFloat()); + xz.push_back(npos.Y.ToFloat()); +- SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false); ++ SimRender::ConstructLineOnGround(m_Pathfinder.GetSimContext(), xz, m_Pathfinder.m_DebugOverlayShortPathLines.back(), false); ++ lock.unlock(); + //*/ if (visible) { @@ -927,5 +937,5 @@ for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred) path.m_Waypoints.emplace_back(Waypoint{ vertexes[id].p.X, vertexes[id].p.Y }); - PROFILE_END("Short pathfinding - A*"); + //PROFILE_END("Short pathfinding - A*"); } Index: source/simulation2/components/CCmpRallyPointRenderer.cpp =================================================================== --- source/simulation2/components/CCmpRallyPointRenderer.cpp +++ source/simulation2/components/CCmpRallyPointRenderer.cpp @@ -681,7 +681,7 @@ start.X = m_RallyPoints[index-1].X; start.Y = m_RallyPoints[index-1].Y; } - cmpPathfinder->ComputePath(start.X, start.Y, goal, cmpPathfinder->GetPassabilityClass(m_LinePassabilityClass), path); + cmpPathfinder->ComputePathImmediate(start.X, start.Y, goal, cmpPathfinder->GetPassabilityClass(m_LinePassabilityClass), path); // Check if we got a path back; if not we probably have two markers less than one tile apart. if (path.m_Waypoints.size() < 2) Index: source/simulation2/components/CCmpVisualActor.cpp =================================================================== --- source/simulation2/components/CCmpVisualActor.cpp +++ source/simulation2/components/CCmpVisualActor.cpp @@ -68,8 +68,6 @@ private: std::wstring m_BaseActorName, m_ActorName; bool m_IsFoundationActor; - - // Not initialized in non-visual mode CUnit* m_Unit; fixed m_R, m_G, m_B; // shading color @@ -452,13 +450,13 @@ SetVariant("animation", m_AnimName); - CmpPtr cmpSound(GetEntityHandle()); - if (cmpSound) - m_SoundGroup = cmpSound->GetSoundGroup(wstring_from_utf8(m_AnimName)); - if (!m_Unit || !m_Unit->GetAnimation() || !m_Unit->GetID()) return; + CmpPtr cmpSound(GetSimContext(), m_Unit->GetID()); + if (cmpSound) + m_SoundGroup = cmpSound->GetSoundGroup(wstring_from_utf8(m_AnimName)); + m_Unit->GetAnimation()->SetAnimationState(m_AnimName, m_AnimOnce, m_AnimSpeed.ToFloat(), m_AnimDesync.ToFloat(), m_SoundGroup.c_str()); } Index: source/simulation2/components/ICmpPathfinder.h =================================================================== --- source/simulation2/components/ICmpPathfinder.h +++ source/simulation2/components/ICmpPathfinder.h @@ -33,6 +33,16 @@ template class Grid; +// returned by asynchronous workers, used to send messages in the main thread. +struct WaypointPath; + +struct PathResult +{ + u32 ticket; + entity_id_t notify; + WaypointPath path; +}; + /** * Pathfinder algorithms. * @@ -89,35 +99,25 @@ 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) = 0; - - /** - * Asynchronous version of ComputePath. + * Request a long path computation, asynchronously. * 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. + /* + * Request a long-path computation immediately */ - virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) = 0; + virtual void ComputePathImmediate(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) const = 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. + * If the debug overlay is enabled, render the path that will computed by ComputePath. */ - virtual void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) = 0; + virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) = 0; /** - * Asynchronous version of ComputeShortPath (using ControlGroupObstructionFilter). + * Request a short path computation, asynchronously. * 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. @@ -171,12 +171,12 @@ /** * Finish computing asynchronous path requests and send the CMessagePathResult messages. */ - virtual void FinishAsyncRequests() = 0; + virtual void FetchAsyncResultsAndSendMessages() = 0; /** - * Process moves during the same turn they were created in to improve responsiveness. + * Tell asynchronous pathfinder threads that they can begin computing paths. */ - virtual void ProcessSameTurnMoves() = 0; + virtual void StartProcessingMoves(bool useMax = false) = 0; /** * Regenerates the grid based on the current obstruction list, if necessary Index: source/simulation2/components/tests/test_Pathfinder.h =================================================================== --- source/simulation2/components/tests/test_Pathfinder.h +++ source/simulation2/components/tests/test_Pathfinder.h @@ -158,7 +158,7 @@ PathGoal goal = { PathGoal::POINT, x1, z1 }; WaypointPath path; - cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), path); + cmp->ComputePathImmediate(x0, z0, goal, cmp->GetPassabilityClass("default"), path); } t = timer_Time() - t; @@ -191,7 +191,7 @@ NullObstructionFilter filter; PathGoal goal = { PathGoal::POINT, range, range }; WaypointPath path; - cmpPathfinder->ComputeShortPath(filter, range/3, range/3, fixed::FromInt(2), range, goal, 0, path); + //cmpPathfinder->ComputeShortPath(filter, range/3, range/3, fixed::FromInt(2), range, goal, 0, path); 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()); } @@ -342,7 +342,7 @@ PathGoal goal = { PathGoal::POINT, x1, z1 }; WaypointPath path; - cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path); + cmpPathfinder->ComputePathImmediate(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path); u32 debugSteps; double debugTime; @@ -391,7 +391,7 @@ for (int i = 0; i < n; ++i) { WaypointPath path; - cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path); + cmpPathfinder->ComputePathImmediate(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path); } t = timer_Time() - t; debug_printf("### RepeatPath %fms each (%fs total)\n", 1000*t / n, t); Index: source/simulation2/helpers/HierarchicalPathfinder.h =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.h +++ source/simulation2/helpers/HierarchicalPathfinder.h @@ -86,7 +86,7 @@ void Update(Grid* grid, const Grid& dirtinessGrid); - RegionID Get(u16 i, u16 j, pass_class_t passClass); + RegionID Get(u16 i, u16 j, pass_class_t passClass) const; /** * Updates @p goal so that it's guaranteed to be reachable from the navcell @@ -98,18 +98,18 @@ * In the case of a non-point reachable goal, it is replaced with a point goal * at the reachable navcell of the goal which is nearest to the starting navcell. */ - void MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass); + void MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const; /** * Updates @p i, @p j (which is assumed to be an impassable navcell) * to the nearest passable navcell. */ - void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass); + void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const; /** * Generates the connectivity grid associated with the given pass_class */ - Grid GetConnectivityGrid(pass_class_t passClass); + Grid GetConnectivityGrid(pass_class_t passClass) const; pass_class_t GetPassabilityClass(const std::string& name) const { @@ -146,25 +146,25 @@ typedef std::map > EdgesMap; - void FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges); + void FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) const; - void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass); + void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const; - void FindPassableRegions(std::set& regions, pass_class_t passClass); + void FindPassableRegions(std::set& regions, pass_class_t passClass) const; /** * Updates @p iGoal and @p jGoal to the navcell that is the nearest to the * initial goal coordinates, in one of the given @p regions. * (Assumes @p regions is non-empty.) */ - void FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass); + void FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const; - Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) + const Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) const { - return m_Chunks[passClass].at(cj * m_ChunksW + ci); + return m_Chunks.at(passClass).at(cj * m_ChunksW + ci); } - void FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid); + void FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) const; u16 m_W, m_H; u16 m_ChunksW, m_ChunksH; Index: source/simulation2/helpers/HierarchicalPathfinder.cpp =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.cpp +++ source/simulation2/helpers/HierarchicalPathfinder.cpp @@ -351,7 +351,7 @@ const std::map& nonPathfindingPassClassMasks, const std::map& pathfindingPassClassMasks) { - PROFILE3("Hierarchical Recompute"); + //PROFILE3("Hierarchical Recompute"); m_PassClassMasks = pathfindingPassClassMasks; @@ -399,7 +399,7 @@ if (m_DebugOverlay) { - PROFILE("debug overlay"); + //PROFILE("debug overlay"); m_DebugOverlayLines.clear(); AddDebugEdges(GetPassabilityClass("default")); } @@ -407,7 +407,7 @@ void HierarchicalPathfinder::Update(Grid* grid, const Grid& dirtinessGrid) { - PROFILE3("Hierarchical Update"); + //PROFILE3("Hierarchical Update"); for (int cj = 0; cj < m_ChunksH; ++cj) { @@ -448,7 +448,7 @@ if (m_DebugOverlay) { - PROFILE("debug overlay"); + //PROFILE("debug overlay"); m_DebugOverlayLines.clear(); AddDebugEdges(GetPassabilityClass("default")); } @@ -457,11 +457,11 @@ /** * Find edges between regions in this chunk and the adjacent below/left chunks. */ -void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) +void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) const { - std::vector& chunks = m_Chunks[passClass]; + const std::vector& chunks = m_Chunks.at(passClass); - Chunk& a = chunks.at(cj*m_ChunksW + ci); + const Chunk& a = chunks.at(cj*m_ChunksW + ci); // For each edge between chunks, we loop over every adjacent pair of // navcells in the two chunks. If they are both in valid regions @@ -473,7 +473,7 @@ if (ci > 0) { - Chunk& b = chunks.at(cj*m_ChunksW + (ci-1)); + const Chunk& b = chunks.at(cj*m_ChunksW + (ci-1)); RegionID raPrev(0,0,0); RegionID rbPrev(0,0,0); for (int j = 0; j < CHUNK_SIZE; ++j) @@ -494,7 +494,7 @@ if (cj > 0) { - Chunk& b = chunks.at((cj-1)*m_ChunksW + ci); + const Chunk& b = chunks.at((cj-1)*m_ChunksW + ci); RegionID raPrev(0,0,0); RegionID rbPrev(0,0,0); for (int i = 0; i < CHUNK_SIZE; ++i) @@ -556,17 +556,17 @@ } } -HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) +HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) const { int ci = i / CHUNK_SIZE; int cj = j / CHUNK_SIZE; ENSURE(ci < m_ChunksW && cj < m_ChunksH); - return m_Chunks[passClass][cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE); + return m_Chunks.at(passClass)[cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE); } -void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) +void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const { - PROFILE2("MakeGoalReachable"); + //PROFILE2("MakeGoalReachable"); RegionID source = Get(i0, j0, passClass); // Find everywhere that's reachable @@ -633,14 +633,14 @@ goal = newGoal; } -void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) +void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const { std::set regions; FindPassableRegions(regions, passClass); FindNearestNavcellInRegions(regions, i, j, passClass); } -void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) +void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const { // Find the navcell in the given regions that's nearest to the goal navcell: // * For each region, record the (squared) minimal distance to the goal point @@ -697,7 +697,7 @@ jGoal = jBest; } -void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) +void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const { // Flood-fill the region graph, starting at 'from', // collecting all the regions that are reachable via edges @@ -711,7 +711,13 @@ RegionID curr = open.back(); open.pop_back(); - for (const RegionID& region : m_Edges[passClass][curr]) + // apparently the region we're from has no edges, so we'll return the empty set + // TODO: maybe here we should pretend we are on any other region? + // indeed because of rasterization issues sometimes we can end up on "unconnected" regions + if (m_Edges.at(passClass).find(curr) == m_Edges.at(passClass).end()) + continue; + + for (const RegionID& region : m_Edges.at(passClass).at(curr)) // Add to the reachable set; if this is the first time we added // it then also add it to the open list if (reachable.insert(region).second) @@ -719,10 +725,10 @@ } } -void HierarchicalPathfinder::FindPassableRegions(std::set& regions, pass_class_t passClass) +void HierarchicalPathfinder::FindPassableRegions(std::set& regions, pass_class_t passClass) const { // Construct a set of all regions of all chunks for this pass class - for (const Chunk& chunk : m_Chunks[passClass]) + for (const Chunk& chunk : m_Chunks.at(passClass)) { // region 0 is impassable tiles for (int r = 1; r <= chunk.m_NumRegions; ++r) @@ -730,14 +736,14 @@ } } -void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) +void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) const { ENSURE(grid.m_W == m_W && grid.m_H == m_H); int i0 = region.ci * CHUNK_SIZE; int j0 = region.cj * CHUNK_SIZE; - const Chunk& c = m_Chunks[passClass][region.cj * m_ChunksW + region.ci]; + const Chunk& c = m_Chunks.at(passClass)[region.cj * m_ChunksW + region.ci]; for (int j = 0; j < CHUNK_SIZE; ++j) for (int i = 0; i < CHUNK_SIZE; ++i) @@ -745,7 +751,7 @@ grid.set(i0 + i, j0 + j, value); } -Grid HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) +Grid HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) const { Grid connectivityGrid(m_W, m_H); connectivityGrid.reset(); Index: source/simulation2/helpers/LongPathfinder.h =================================================================== --- source/simulation2/helpers/LongPathfinder.h +++ source/simulation2/helpers/LongPathfinder.h @@ -18,6 +18,7 @@ #ifndef INCLUDED_LONGPATHFINDER #define INCLUDED_LONGPATHFINDER +#include #include "Pathfinding.h" #include "HierarchicalPathfinder.h" @@ -217,7 +218,7 @@ * along the path. */ void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, - pass_class_t passClass, WaypointPath& path) + pass_class_t passClass, WaypointPath& path) const { if (!m_Grid) { @@ -251,33 +252,37 @@ u16 m_GridSize; // Debugging - output from last pathfind operation: - LongOverlay* m_DebugOverlay; - PathfindTileGrid* m_DebugGrid; - u32 m_DebugSteps; - double m_DebugTime; - PathGoal m_DebugGoal; - WaypointPath* m_DebugPath; - pass_class_t m_DebugPassClass; + // HACK: computeJPSpath is const to ensure it's read-only but we want to be able to debug + // so mark those as mutable and use a mutex to lock. + mutable LongOverlay* m_DebugOverlay; + mutable PathfindTileGrid* m_DebugGrid; + mutable u32 m_DebugSteps; + mutable double m_DebugTime; + mutable PathGoal m_DebugGoal; + mutable WaypointPath* m_DebugPath; + mutable pass_class_t m_DebugPassClass; + + mutable std::mutex m_Mutex; private: - PathCost CalculateHeuristic(int i, int j, int iGoal, int jGoal); - void ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state); + PathCost CalculateHeuristic(int i, int j, int iGoal, int jGoal) const; + void ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state) const; /** * JPS algorithm helper functions * @param detectGoal is not used if m_UseJPSCache is true */ - void AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal); - int HasJumpedHoriz(int i, int j, int di, PathfinderState& state, bool detectGoal); - void AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal); - int HasJumpedVert(int i, int j, int dj, PathfinderState& state, bool detectGoal); - void AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderState& state); + void AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal) const; + int HasJumpedHoriz(int i, int j, int di, PathfinderState& state, bool detectGoal) const; + void AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal) const; + int HasJumpedVert(int i, int j, int dj, PathfinderState& state, bool detectGoal) const; + void AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderState& state) const; /** * See LongPathfinder.cpp for implementation details * TODO: cleanup documentation */ - void ComputeJPSPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path); + void ComputeJPSPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path)const ; void GetDebugDataJPS(u32& steps, double& time, Grid& grid) const; // Helper functions for ComputePath @@ -290,7 +295,7 @@ * If @param maxDist is non-zero, path waypoints will be espaced by at most @param maxDist. * In that case the distance between (x0, z0) and the first waypoint will also be made less than maxDist. */ - void ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0); + void ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0) const; /** * Generate a passability map, stored in the 16th bit of navcells, based on passClass, Index: source/simulation2/helpers/LongPathfinder.cpp =================================================================== --- source/simulation2/helpers/LongPathfinder.cpp +++ source/simulation2/helpers/LongPathfinder.cpp @@ -289,7 +289,7 @@ void reset(const Grid* terrain, pass_class_t passClass) { - PROFILE3("JumpPointCache reset"); + //PROFILE3("JumpPointCache reset"); TIMER(L"JumpPointCache reset"); m_Width = terrain->m_W; @@ -388,7 +388,7 @@ // Calculate heuristic cost from tile i,j to goal // (This ought to be an underestimate for correctness) -PathCost LongPathfinder::CalculateHeuristic(int i, int j, int iGoal, int jGoal) +PathCost LongPathfinder::CalculateHeuristic(int i, int j, int iGoal, int jGoal) const { int di = abs(i - iGoal); int dj = abs(j - jGoal); @@ -397,7 +397,7 @@ } // Do the A* processing for a neighbour tile i,j. -void LongPathfinder::ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state) +void LongPathfinder::ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state) const { // Reject impassable tiles if (!PASSABLE(i, j)) @@ -501,7 +501,7 @@ } -void LongPathfinder::AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal) +void LongPathfinder::AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal) const { if (m_UseJPSCache) { @@ -543,7 +543,7 @@ } // Returns the i-coordinate of the jump point if it exists, else returns i -int LongPathfinder::HasJumpedHoriz(int i, int j, int di, PathfinderState& state, bool detectGoal) +int LongPathfinder::HasJumpedHoriz(int i, int j, int di, PathfinderState& state, bool detectGoal) const { if (m_UseJPSCache) { @@ -579,7 +579,7 @@ } } -void LongPathfinder::AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal) +void LongPathfinder::AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal) const { if (m_UseJPSCache) { @@ -621,7 +621,7 @@ } // Returns the j-coordinate of the jump point if it exists, else returns j -int LongPathfinder::HasJumpedVert(int i, int j, int dj, PathfinderState& state, bool detectGoal) +int LongPathfinder::HasJumpedVert(int i, int j, int dj, PathfinderState& state, bool detectGoal) const { if (m_UseJPSCache) { @@ -661,7 +661,7 @@ * We never cache diagonal jump points - they're usually so frequent that * a linear search is about as cheap and avoids the setup cost and memory cost. */ -void LongPathfinder::AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderState& state) +void LongPathfinder::AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderState& state) const { // ProcessNeighbour(i, j, i + di, j + dj, g, state); // return; @@ -712,13 +712,15 @@ } } -void LongPathfinder::ComputeJPSPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path) +void LongPathfinder::ComputeJPSPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path) const { - PROFILE("ComputePathJPS"); - PROFILE2_IFSPIKE("ComputePathJPS", 0.0002); + //PROFILE("ComputePathJPS"); + //PROFILE2("ComputePathJPS"); PathfinderState state = { 0 }; - state.jpc = m_JumpPointCache[passClass].get(); + ENSURE(!m_UseJPSCache && "Jump Point Cache currently not supported as it must handle threading properly."); + /* + state.jpc = m_JumpPointCache.at(passClass).get(); if (m_UseJPSCache && !state.jpc) { state.jpc = new JumpPointCache; @@ -726,7 +728,7 @@ debug_printf("PATHFINDER: JPC memory: %d kB\n", (int)state.jpc->GetMemoryUsage() / 1024); m_JumpPointCache[passClass] = shared_ptr(state.jpc); } - + */ // Convert the start coordinates to tile indexes u16 i0, j0; Pathfinding::NearestNavcell(x0, z0, i0, j0, m_GridSize, m_GridSize); @@ -898,14 +900,14 @@ // Save this grid for debug display delete m_DebugGrid; - m_DebugGrid = state.tiles; - m_DebugSteps = state.steps; - m_DebugGoal = state.goal; + //m_DebugGrid = state.tiles; + //m_DebugSteps = state.steps; + //m_DebugGoal = state.goal; } #undef PASSABLE -void LongPathfinder::ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0) +void LongPathfinder::ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0) const { if (path.m_Waypoints.empty()) return; @@ -959,6 +961,7 @@ void LongPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid& grid) const { + std::unique_lock lock(m_Mutex); steps = m_DebugSteps; time = m_DebugTime; @@ -979,14 +982,17 @@ grid.set(i, j, (t.IsOpen() ? 1 : 0) | (t.IsClosed() ? 2 : 0)); } } + lock.unlock(); } void LongPathfinder::SetDebugOverlay(bool enabled) { + std::unique_lock lock(m_Mutex); if (enabled && !m_DebugOverlay) m_DebugOverlay = new LongOverlay(*this); else if (!enabled && m_DebugOverlay) SAFE_DELETE(m_DebugOverlay); + lock.unlock(); } void LongPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, Index: source/simulation2/helpers/Render.cpp =================================================================== --- source/simulation2/helpers/Render.cpp +++ source/simulation2/helpers/Render.cpp @@ -35,8 +35,6 @@ void SimRender::ConstructLineOnGround(const CSimContext& context, const std::vector& xz, SOverlayLine& overlay, bool floating, float heightOffset) { - PROFILE("ConstructLineOnGround"); - overlay.m_Coords.clear(); CmpPtr cmpTerrain(context, SYSTEM_ENTITY); @@ -343,7 +341,7 @@ void SimRender::SmoothPointsAverage(std::vector& points, bool closed) { - PROFILE("SmoothPointsAverage"); + //PROFILE("SmoothPointsAverage"); size_t n = points.size(); if (n < 2) @@ -385,7 +383,7 @@ void SimRender::InterpolatePointsRNS(std::vector& points, bool closed, float offset, int segmentSamples /* = 4 */) { - PROFILE("InterpolatePointsRNS"); + //PROFILE("InterpolatePointsRNS"); ENSURE(segmentSamples > 0); std::vector newPoints;