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/simulation2/Simulation2.cpp =================================================================== --- source/simulation2/Simulation2.cpp +++ source/simulation2/Simulation2.cpp @@ -515,8 +515,8 @@ CmpPtr cmpPathfinder(simContext, SYSTEM_ENTITY); if (cmpPathfinder) { + cmpPathfinder->FetchAsyncResultsAndSendMessages(); cmpPathfinder->UpdateGrid(); - cmpPathfinder->FinishAsyncRequests(); } // Push AI commands onto the queue before we use them @@ -529,24 +529,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); @@ -558,12 +559,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/CCmpObstructionManager.cpp =================================================================== --- source/simulation2/components/CCmpObstructionManager.cpp +++ source/simulation2/components/CCmpObstructionManager.cpp @@ -470,9 +470,9 @@ virtual bool TestUnitShape(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, std::vector* out); virtual void Rasterize(Grid& grid, const std::vector& passClasses, bool fullUpdate); - virtual void GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares); - virtual void GetUnitObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares); - virtual void GetStaticObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares); + virtual void GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const; + virtual void GetUnitObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const; + virtual void GetStaticObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const; virtual void GetUnitsOnObstruction(const ObstructionSquare& square, std::vector& out, const IObstructionTestFilter& filter, bool strict = false); virtual void SetPassabilityCircular(bool enabled) @@ -928,16 +928,14 @@ } } -void CCmpObstructionManager::GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) +void CCmpObstructionManager::GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const { GetUnitObstructionsInRange(filter, x0, z0, x1, z1, squares); GetStaticObstructionsInRange(filter, x0, z0, x1, z1, squares); } -void CCmpObstructionManager::GetUnitObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) +void CCmpObstructionManager::GetUnitObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const { - PROFILE("GetObstructionsInRange"); - ENSURE(x0 <= x1 && z0 <= z1); std::vector unitShapes; @@ -962,10 +960,8 @@ } } -void CCmpObstructionManager::GetStaticObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) +void CCmpObstructionManager::GetStaticObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const { - PROFILE("GetObstructionsInRange"); - ENSURE(x0 <= x1 && z0 <= z1); std::vector staticShapes; Index: source/simulation2/components/CCmpPathfinder.cpp =================================================================== --- source/simulation2/components/CCmpPathfinder.cpp +++ source/simulation2/components/CCmpPathfinder.cpp @@ -37,6 +37,8 @@ #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)) @@ -53,8 +55,6 @@ m_DebugOverlay = false; m_AtlasOverlay = NULL; - m_SameTurnMovesCount = 0; - // Register Relax NG validator CXeromyces::AddValidator(g_VFS, "pathfinder", "simulation/data/pathfinder.rng"); @@ -64,25 +64,9 @@ 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) { @@ -92,10 +76,35 @@ m_PassClasses.push_back(PathfinderPassability(mask, it->second)); m_PassClassMasks[name] = mask; } + + // 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() { + 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); @@ -141,7 +150,6 @@ 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); } @@ -177,9 +185,6 @@ UpdateGrid(); m_PreserveUpdateInformations = true; break; - case MT_TurnStart: - m_SameTurnMovesCount = 0; - break; } } @@ -670,123 +675,250 @@ ////////////////////////////////////////////////////////// -// 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) + g_Profiler2.RegisterCurrentThread("Pathfinder thread " + std::to_string(index)); + WaitForWork(); +} - ProcessLongRequests(longRequests); - ProcessShortRequests(shortRequests); +void CCmpPathfinder::AsyncPathfinderWorkerThread::PrepareToKill() +{ + m_Kill = true; } -void CCmpPathfinder::ProcessLongRequests(const std::vector& longRequests) +void CCmpPathfinder::AsyncPathfinderWorkerThread::WaitForWork() { - PROFILE2("Process Long Requests"); - for (size_t i = 0; i < longRequests.size(); ++i) + while (!m_Kill) { - 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); + // 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::ProcessShortRequests(const std::vector& shortRequests) +void CCmpPathfinder::AsyncPathfinderWorkerThread::Work() { - PROFILE2("Process Short Requests"); - for (size_t i = 0; i < shortRequests.size(); ++i) + m_Computing = true; + + // start by going through our long requests. + while (!m_LongRequests.empty()) { - const AsyncShortPathRequest& req = shortRequests[i]; + std::unique_lock lock(m_Pause); + if (!m_Pathfinder.m_MayComputePaths) + return; + + 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}); + + 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; + + 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); - CMessagePathResult msg(req.ticket, path); - GetSimContext().GetComponentManager().PostMessage(req.notify, msg); + m_Results.push_back({req.ticket, req.notify, path}); + + m_ShortRequests.pop_back(); + lock.unlock(); } + std::unique_lock lock(m_Pause); + m_Computing = false; + lock.unlock(); } -void CCmpPathfinder::ProcessSameTurnMoves() +// Async path requests: + +u32 CCmpPathfinder::ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) { - if (!m_AsyncLongPathRequests.empty()) - { - // Figure out how many moves we can do this time - i32 moveCount = m_MaxSameTurnMoves - m_SameTurnMovesCount; + AsyncLongPathRequest req = { m_NextAsyncTicket++, x0, z0, goal, passClass, notify }; + m_AsyncLongPathRequests.push_back(req); + return req.ticket; +} - if (moveCount <= 0) - return; +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); +} - // Copy the long request elements we are going to process into a new array - std::vector longRequests; - if ((i32)m_AsyncLongPathRequests.size() <= moveCount) +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; + 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) { - m_AsyncLongPathRequests.swap(longRequests); - moveCount = (i32)longRequests.size(); + CMessagePathResult msg(path.ticket, path.path); + GetSimContext().GetComponentManager().PostMessage(path.notify, msg); } - else + } +} + +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.resize(moveCount); - copy(m_AsyncLongPathRequests.begin(), m_AsyncLongPathRequests.begin() + moveCount, longRequests.begin()); - m_AsyncLongPathRequests.erase(m_AsyncLongPathRequests.begin(), m_AsyncLongPathRequests.begin() + moveCount); + 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(); + } - ProcessLongRequests(longRequests); + done += longRequests.size(); - m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); + 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(); } - if (!m_AsyncShortPathRequests.empty()) + std::vector shortRequests; + if (useMax) { - // Figure out how many moves we can do now - i32 moveCount = m_MaxSameTurnMoves - m_SameTurnMovesCount; - - if (moveCount <= 0) - return; - - // Copy the short request elements we are going to process into a new array - std::vector shortRequests; - if ((i32)m_AsyncShortPathRequests.size() <= moveCount) + size_t amount = std::min(m_AsyncShortPathRequests.size(), (size_t)m_MaxSameTurnMoves-done); + if (amount > 0) { - m_AsyncShortPathRequests.swap(shortRequests); - moveCount = (i32)shortRequests.size(); + 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 + } + 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) { - shortRequests.resize(moveCount); - copy(m_AsyncShortPathRequests.begin(), m_AsyncShortPathRequests.begin() + moveCount, shortRequests.begin()); - m_AsyncShortPathRequests.erase(m_AsyncShortPathRequests.begin(), m_AsyncShortPathRequests.begin() + moveCount); + 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(); + } - ProcessShortRequests(shortRequests); - - m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); + // 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(); } ////////////////////////////////////////////////////////// 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_PreserveUpdateInformations; // 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() @@ -237,17 +321,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); @@ -279,13 +358,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); - 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 }; @@ -852,16 +857,19 @@ CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) && 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::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 +935,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 @@ -636,7 +636,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/ICmpObstructionManager.h =================================================================== --- source/simulation2/components/ICmpObstructionManager.h +++ source/simulation2/components/ICmpObstructionManager.h @@ -237,9 +237,9 @@ * @param z1 Z coordinate of top edge of range * @param squares output list of obstructions */ - virtual void GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) = 0; - virtual void GetStaticObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) = 0; - virtual void GetUnitObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) = 0; + virtual void GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const = 0; + virtual void GetStaticObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const = 0; + virtual void GetUnitObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const = 0; /** * Returns the entity IDs of all unit shapes that intersect the given 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/helpers/HierarchicalPathfinder.h =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.h +++ source/simulation2/helpers/HierarchicalPathfinder.h @@ -88,7 +88,7 @@ bool IsChunkDirty(int ci, int cj, const Grid& dirtinessGrid) const; - 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 @@ -100,18 +100,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 { @@ -148,25 +148,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 @@ -464,11 +464,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 @@ -480,7 +480,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) @@ -501,7 +501,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) @@ -563,15 +563,15 @@ } } -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"); RegionID source = Get(i0, j0, passClass); @@ -640,14 +640,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 @@ -704,7 +704,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 @@ -718,7 +718,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) @@ -726,10 +732,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) @@ -737,14 +743,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) @@ -752,7 +758,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 @@ -219,7 +219,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) { @@ -253,33 +253,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); // Helper functions for ComputePath @@ -292,7 +296,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 @@ -393,7 +393,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); @@ -402,7 +402,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)) @@ -538,7 +538,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) { @@ -585,7 +585,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) { @@ -626,7 +626,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) { @@ -673,7 +673,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) { @@ -718,7 +718,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; @@ -782,13 +782,15 @@ #undef PASSABLE -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; @@ -796,7 +798,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); @@ -982,13 +984,16 @@ ImprovePathWaypoints(path, passClass, origGoal.maxdist, x0, z0); // Save this grid for debug display + //TODO: reimplement this after threading + std::unique_lock lock(m_Mutex); delete m_DebugGrid; m_DebugGrid = state.tiles; m_DebugSteps = state.steps; m_DebugGoal = state.goal; + lock.unlock(); } -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; @@ -1042,6 +1047,7 @@ void LongPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid& grid) { + std::unique_lock lock(m_Mutex); steps = m_DebugSteps; time = m_DebugTime; @@ -1062,14 +1068,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);