Index: binaries/data/config/default.cfg =================================================================== --- binaries/data/config/default.cfg +++ binaries/data/config/default.cfg @@ -403,6 +403,10 @@ zoom.in = 5 zoom.out = 4 +[pathfinder] +usenumthreads = false; +numthreads = 0; + [chat] timestamp = true ; Show at which time chat messages have been sent Index: binaries/data/mods/public/gui/credits/texts/programming.json =================================================================== --- binaries/data/mods/public/gui/credits/texts/programming.json +++ binaries/data/mods/public/gui/credits/texts/programming.json @@ -122,6 +122,7 @@ {"nick": "kabzerek", "name": "Grzegorz Kabza"}, {"nick": "Kai", "name": "Kai Chen"}, {"name": "Kareem Ergawy"}, + {"nick": "Kuba386", "name":"Jakub Kośmicki"}, {"nick": "kevmo", "name": "Kevin Caffrey"}, {"nick": "kezz", "name": "Graeme Kerry"}, {"nick": "kingadami", "name": "Adam Winsor"}, 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,6 +81,21 @@ "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", + "tooltip":"Use default thread count or choose your own.", + "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 } ] }, 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 @@ -1,4 +1,4 @@ -/* Copyright (C) 2013 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -175,16 +175,22 @@ public: CProfileSample(const char* name) { - if (CProfileManager::IsInitialised()) - { - // The profiler is only safe to use on the main thread - ENSURE(ThreadUtil::IsMainThread()); + if (!CProfileManager::IsInitialised()) + return; - g_Profiler.Start(name); - } + // The profiler is only safe to use on the main thread + + if(!ThreadUtil::IsMainThread()) + return; + + g_Profiler.Start(name); } ~CProfileSample() { + + if(!ThreadUtil::IsMainThread()) + return; + if (CProfileManager::IsInitialised()) g_Profiler.Stop(); } Index: source/simulation2/Simulation2.cpp =================================================================== --- source/simulation2/Simulation2.cpp +++ source/simulation2/Simulation2.cpp @@ -531,17 +531,18 @@ CComponentManager& componentManager = simContext.GetComponentManager(); - { - PROFILE2("Sim - Update Start"); - CMessageTurnStart msgTurnStart; - componentManager.BroadcastMessage(msgTurnStart); - } - CmpPtr cmpPathfinder(simContext, SYSTEM_ENTITY); + // Collect results of background computations we have started previous turn if (cmpPathfinder) { + cmpPathfinder->FetchAsyncResultsAndSendMessages(); cmpPathfinder->UpdateGrid(); - cmpPathfinder->FinishAsyncRequests(); + } + + { + PROFILE2("Sim - Update Start"); + CMessageTurnStart msgTurnStart; + componentManager.BroadcastMessage(msgTurnStart); } // Push AI commands onto the queue before we use them @@ -555,40 +556,40 @@ // Process newly generated move commands so the UI feels snappy 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); componentManager.BroadcastMessage(msgUpdate); } + { PROFILE2("Sim - Update Final"); CMessageUpdate_Final msgUpdate(turnLengthFixed); 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(); + + // Start processing all remaining moves in background + if (cmpPathfinder) + cmpPathfinder->StartProcessingMoves(); } void CSimulation2Impl::Interpolate(float simFrameLength, float frameOffset, float realFrameLength) Index: source/simulation2/components/CCmpAIManager.cpp =================================================================== --- source/simulation2/components/CCmpAIManager.cpp +++ source/simulation2/components/CCmpAIManager.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2018 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -245,6 +245,8 @@ ~CAIWorker() { JS_RemoveExtraGCRootsTracer(m_ScriptInterface->GetJSRuntime(), Trace, this); + m_ScriptInterface.reset(); + m_SerializablePrototypes.reset(); } bool HasLoadedEntityTemplates() const { return m_HasLoadedEntityTemplates; } Index: source/simulation2/components/CCmpPathfinder.cpp =================================================================== --- source/simulation2/components/CCmpPathfinder.cpp +++ source/simulation2/components/CCmpPathfinder.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2018 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -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,6 +38,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)) @@ -52,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"); @@ -63,38 +64,81 @@ 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(); - + m_MaxSameTurnMoves = static_cast(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); + ENSURE(static_cast(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; } + + // This is to stop tests from crashing. + if(!CConfigDB::IsInitialised()) + { + new CConfigDB; + } + + // Read thread count preferences. If they aren't specified use defaults. + + bool useCustomThreadCount = false; +#ifndef TEST + CFG_GET_VAL("pathfinder.usenumthreads", useCustomThreadCount); +#endif + if (useCustomThreadCount) + { + int threadcount; + CFG_GET_VAL("pathfinder.numthreads", threadcount); + + if (threadcount < 1 || (g_AtlasGameLoop && g_AtlasGameLoop->running) || threadcount > 64) + { + useCustomThreadCount = false; + } + else + { + for (u8 i = 0; i < static_cast(threadcount); ++i) + m_WorkerThreads.push_back(new AsyncPathfinderWorkerThread(*this)); + } + } + if (!useCustomThreadCount) + { + // 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 (AsyncPathfinderWorkerThread* worker : m_WorkerThreads) + worker->PrepareToKill(); + m_MainThreadSignal.notify_all(); + for (AsyncPathfinderWorkerThread* worker : m_WorkerThreads) + delete worker; + + m_WorkerThreads.clear(); SetDebugOverlay(false); // cleans up memory SAFE_DELETE(m_AtlasOverlay); @@ -140,7 +184,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); } @@ -175,9 +218,6 @@ m_TerrainDirty = true; UpdateGrid(); break; - case MT_TurnStart: - m_SameTurnMovesCount = 0; - break; } } @@ -237,7 +277,7 @@ return &passability; } - return NULL; + return nullptr; } const Grid& CCmpPathfinder::GetPassabilityGrid() @@ -357,8 +397,9 @@ 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)) + 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); @@ -463,7 +504,7 @@ 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_DirtinessInformation = {true, true, Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE)}; m_AIPathfinderDirtinessInformation = m_DirtinessInformation; m_TerrainDirty = true; @@ -558,8 +599,7 @@ { 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_DirtinessInformation = {true, true, Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE)}; m_AIPathfinderDirtinessInformation = m_DirtinessInformation; } } @@ -670,125 +710,291 @@ } } -////////////////////////////////////////////////////////// +bool CCmpPathfinder::MakeGoalReachable(entity_pos_t x0, entity_pos_t z0, PathGoal &goal, pass_class_t passClass) +{ + u16 i0, j0; + Pathfinding::NearestNavcell(x0, z0, i0, j0, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); + if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass)) + FindNearestPassableNavcell(i0, j0, passClass); -// Async path requests: + return m_LongPathfinder.MakeGoalReachable(i0, j0, goal, passClass); +} -u32 CCmpPathfinder::ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) +u32 CCmpPathfinder::FindNearestPassableNavcell(entity_pos_t x, entity_pos_t z, u16& outI, u16& outJ, pass_class_t passClass) { - AsyncLongPathRequest req = { m_NextAsyncTicket++, x0, z0, goal, passClass, notify }; - m_AsyncLongPathRequests.push_back(req); - return req.ticket; + Pathfinding::NearestNavcell(x, z, outI, outJ, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); + u16 i0 = outI; + u16 j0 = outJ; + FindNearestPassableNavcell(outI, outJ, passClass); + return abs(i0 - outI) + abs(j0 - outJ); } -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) +void CCmpPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) { - AsyncShortPathRequest req = { m_NextAsyncTicket++, x0, z0, clearance, range, goal, passClass, avoidMovingUnits, group, notify }; - m_AsyncShortPathRequests.push_back(req); - return req.ticket; + m_LongPathfinder.FindNearestPassableNavcell(i, j, passClass); } -void CCmpPathfinder::FinishAsyncRequests() +bool CCmpPathfinder::NavcellIsReachable(u16 i0, u16 j0, u16 i1, u16 j1, pass_class_t passClass) { - PROFILE2("Finish Async Requests"); - // Save the request queue in case it gets modified while iterating - std::vector longRequests; - m_AsyncLongPathRequests.swap(longRequests); + return m_LongPathfinder.NavcellIsReachable(i0, j0, i1, j1, passClass); +} - std::vector shortRequests; - m_AsyncShortPathRequests.swap(shortRequests); +////////////////////////////////////////////////////////// - // TODO: we should only compute one path per entity per turn +// Async pathfinder workers - // TODO: this computation should be done incrementally, spread - // across multiple frames (or even multiple turns) +CCmpPathfinder::AsyncPathfinderWorkerThread::AsyncPathfinderWorkerThread(const CCmpPathfinder& pathfinder) + : m_Pathfinder(pathfinder) +{ + if (m_Pathfinder.m_UseThreading) + m_Thread = new std::thread(&CCmpPathfinder::AsyncPathfinderWorkerThread::InitThread, this, + m_Pathfinder.m_WorkerThreads.size()); +} - ProcessLongRequests(longRequests); - ProcessShortRequests(shortRequests); +CCmpPathfinder::AsyncPathfinderWorkerThread::~AsyncPathfinderWorkerThread() +{ + if (m_Thread != nullptr) + { + if (m_Thread->joinable()) + m_Thread->join(); + delete m_Thread; + } +} + +void CCmpPathfinder::AsyncPathfinderWorkerThread::InitThread(size_t index) +{ + 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) + m_Kill = true; +} + +void CCmpPathfinder::AsyncPathfinderWorkerThread::WaitForWork() +{ + 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) - { - m_AsyncLongPathRequests.swap(longRequests); - moveCount = (i32)longRequests.size(); - } - else +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 (AsyncPathfinderWorkerThread* worker : m_WorkerThreads) + if (worker->m_Computing) + stop = false; + if (stop) + break; + } + + std::vector results; + for (AsyncPathfinderWorkerThread* 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) { - longRequests.resize(moveCount); - copy(m_AsyncLongPathRequests.begin(), m_AsyncLongPathRequests.begin() + moveCount, longRequests.begin()); - m_AsyncLongPathRequests.erase(m_AsyncLongPathRequests.begin(), m_AsyncLongPathRequests.begin() + moveCount); + CMessagePathResult msg(path.ticket, path.path); + GetSimContext().GetComponentManager().PostMessage(path.notify, msg); } + } +} - ProcessLongRequests(longRequests); +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 - m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); - } + size_t done = 0; - if (!m_AsyncShortPathRequests.empty()) + std::vector longRequests; + if (useMax) { - // Figure out how many moves we can do now - i32 moveCount = m_MaxSameTurnMoves - m_SameTurnMovesCount; + 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(); + } - if (moveCount <= 0) - return; + done += longRequests.size(); - // Copy the short request elements we are going to process into a new array - std::vector shortRequests; - if ((i32)m_AsyncShortPathRequests.size() <= 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) { - m_AsyncShortPathRequests.swap(shortRequests); - moveCount = (i32)shortRequests.size(); + 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()); } - else + // 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.resize(moveCount); - copy(m_AsyncShortPathRequests.begin(), m_AsyncShortPathRequests.begin() + moveCount, shortRequests.begin()); - m_AsyncShortPathRequests.erase(m_AsyncShortPathRequests.begin(), m_AsyncShortPathRequests.begin() + moveCount); + 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(); + } - ProcessShortRequests(shortRequests); + if (!shortRequests.empty()) + { + size_t amount = shortRequests.size() / m_WorkerThreads.size(); + // try and divide equally among threads. + for (AsyncPathfinderWorkerThread* worker : m_WorkerThreads) + { + std::unique_lock lock(worker->m_Pause); + // move-insert + worker->m_ShortRequests.insert(worker->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(); + } - m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); + // we may now notify them. + m_MayComputePaths = true; + if (m_UseThreading) + { + // immediately mark them as computing + for (AsyncPathfinderWorkerThread* 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(); } ////////////////////////////////////////////////////////// @@ -812,7 +1018,7 @@ } 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 + 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()); Index: source/simulation2/components/CCmpPathfinder_Common.h =================================================================== --- source/simulation2/components/CCmpPathfinder_Common.h +++ source/simulation2/components/CCmpPathfinder_Common.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2017 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -38,6 +38,10 @@ #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/helpers/LongPathfinder.h" +#include +#include +#include + class SceneCollector; class AtlasOverlay; @@ -120,6 +124,98 @@ */ 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 +239,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 +255,25 @@ bool m_TerrainDirty; // Interface to the long-range pathfinder. - LongPathfinder m_LongPathfinder; - - // For responsiveness we will process some moves in the same turn they were generated in + mutable LongPathfinder m_LongPathfinder; - 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 +337,19 @@ 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 bool MakeGoalReachable(entity_pos_t x0, entity_pos_t z0, PathGoal &goal, pass_class_t passClass); - virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify); + virtual u32 FindNearestPassableNavcell(entity_pos_t x, entity_pos_t z, u16& outI, u16& outJ, pass_class_t passClass); + void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass); + + virtual bool NavcellIsReachable(u16 i0, u16 j0, u16 i1, u16 j1, pass_class_t passClass); - 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 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify); 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,21 +373,21 @@ virtual void SetAtlasOverlay(bool enable, pass_class_t passClass = 0); - virtual bool CheckMovement(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, pass_class_t passClass) const; - - virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint) const; - - virtual ICmpObstruction::EFoundationCheck CheckBuildingPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, entity_id_t id, pass_class_t passClass) const; + virtual bool CheckMovement(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, + entity_pos_t z1, entity_pos_t r, pass_class_t passClass) const; - virtual ICmpObstruction::EFoundationCheck 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 ICmpObstruction::EFoundationCheck + CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint) const; - virtual void FinishAsyncRequests(); + virtual ICmpObstruction::EFoundationCheck + CheckBuildingPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a,entity_pos_t w, entity_pos_t h, entity_id_t id, pass_class_t passClass) const; - void ProcessLongRequests(const std::vector& longRequests); + 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; - 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 @@ -335,7 +429,7 @@ for (size_t i = 0; i < w; ++i) { SColor4ub color(0, 0, 0, 0); - if (!IS_PASSABLE(m_Pathfinder->m_TerrainOnlyGrid->get((int)i, (int)j), m_PassClass)) + if (!IS_PASSABLE(m_Pathfinder->m_TerrainOnlyGrid->get(static_cast(i), static_cast(j)), m_PassClass)) color = SColor4ub(255, 0, 0, 127); *p++ = color.R; Index: source/simulation2/components/CCmpPathfinder_Vertex.cpp =================================================================== --- source/simulation2/components/CCmpPathfinder_Vertex.cpp +++ source/simulation2/components/CCmpPathfinder_Vertex.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2017 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -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: @@ -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,16 @@ 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 }); + + edges.clear(); + edgeSquares.clear(); + vertexes.clear(); + + edgesUnaligned.clear(); + edgesLeft.clear(); + edgesRight.clear(); + edgesBottom.clear(); + edgesTop.clear(); + PROFILE_END("Short pathfinding - A*"); } Index: source/simulation2/components/CCmpRallyPointRenderer.cpp =================================================================== --- source/simulation2/components/CCmpRallyPointRenderer.cpp +++ source/simulation2/components/CCmpRallyPointRenderer.cpp @@ -686,7 +686,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 @@ -1,4 +1,4 @@ -/* Copyright (C) 2018 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -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 @@ -1,4 +1,4 @@ -/* Copyright (C) 2017 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -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,43 @@ 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. + * Transform an arbitrary PathGoal into a reachable Point PathGoal, see Hierarchical Pathfinder for details + * Return true if the goal was reachable originally, false otherwise. */ - virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) = 0; + virtual bool MakeGoalReachable(entity_pos_t x0, entity_pos_t z0, PathGoal &goal, pass_class_t passClass) = 0; /** - * Asynchronous version of ComputePath. + * Gives the closest passable navcell from the given position. + * Returns how many navcells away (manhattan) that navcell is. + */ + virtual u32 FindNearestPassableNavcell(entity_pos_t x, entity_pos_t z, u16& outI, u16& outJ, pass_class_t passClass) = 0; + + /** + * Returns true if navcell (i0, j0) has the same global region ID as navcell (i1, j1). + * i.e. you can reach one from the other. + */ + virtual bool NavcellIsReachable(u16 i0, u16 j0, u16 i1, u16 j1, pass_class_t passClass) = 0; + + /** + * 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 +189,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 @@ -1,4 +1,4 @@ -/* Copyright (C) 2018 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -17,8 +17,11 @@ #include "simulation2/system/ComponentTest.h" +#define TEST + #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/components/ICmpPathfinder.h" +#include "simulation2/components/CCmpPathfinder_Common.h" #include "graphics/MapReader.h" #include "graphics/Terrain.h" @@ -111,6 +114,160 @@ } } + void hierarchical_globalRegions_testmap(std::wstring map) + { + CTerrain terrain; + + CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); + sim2.LoadDefaultScripts(); + sim2.ResetState(); + + CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself + + LDR_BeginRegistering(); + mapReader->LoadMap(map, + sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue, + &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL, + &sim2, &sim2.GetSimContext(), -1, false); + LDR_EndRegistering(); + TS_ASSERT_OK(LDR_NonprogressiveLoad()); + + sim2.PreInitGame(); + sim2.InitGame(); + sim2.Update(0); + + CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY); + + pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default"); + HierarchicalPathfinder& hier = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.GetHierarchicalPathfinder(); + + std::map globalRegions = hier.m_GlobalRegions[obstructionsMask]; + + for (u8 cj = 0; cj < hier.m_ChunksH; cj += 2) + for (u8 ci = 0; ci < hier.m_ChunksW; ci += 2) + for(u16 i : hier.GetChunk(ci, cj, obstructionsMask).m_RegionsID) + { + // Assert that all reachable regions are indeed the same region ID + // This does not highlight that unreachable regions might (wrongly) have the same ID). + std::set reachables; + hier.FindReachableRegions(HierarchicalPathfinder::RegionID{ci, cj, i}, reachables, obstructionsMask); + HierarchicalPathfinder::GlobalRegionID ID = globalRegions[HierarchicalPathfinder::RegionID{ci, cj, i}]; + for (HierarchicalPathfinder::RegionID region : reachables) + TS_ASSERT_EQUALS(ID, globalRegions[region]); + } + } + + void test_hierarchical_globalRegions() + { + // This test validates that the hierarchical's pathfinder global regions are in accordance with its regions + // IE it asserts that, for any two regions A and B of the hierarchical pathfinder, if one can find a path from A to B + // then A and B have the same global region. + std::vector maps = { L"maps/scenarios/Peloponnese.pmp", L"maps/skirmishes/Corinthian Isthmus (2).pmp", L"maps/skirmishes/Greek Acropolis (2).pmp" }; + +// disable in debug mode, creating the simulation and running the initial turn is too slow and tends to OOM in debug mode. +#ifndef DEBUG + for (std::wstring t : maps) + hierarchical_globalRegions_testmap(t); +#endif + } + void test_reachability_sanity_test() + { + // Goal of this test: validate that we stay on the same cell. This isn't 100% coverage but if the point goal gets broken, things will be _bad_. + PathGoal tgoal{PathGoal::POINT, fixed::FromInt(2), fixed::FromInt(2), fixed::Zero(), fixed::Zero(), CFixedVector2D(fixed::FromInt(1), fixed::Zero()),CFixedVector2D(fixed::Zero(), fixed::FromInt(1)), fixed::Zero()}; + + PathGoal ogoal = tgoal; + + // End up on the top left edge. + CreatePointGoalAt(5, 6, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(5)); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(6)); + tgoal = ogoal; + // End up as close as possible from (2,2) on the same cell. + CreatePointGoalAt(1, 1, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(2) - fixed::Epsilon()); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(2) - fixed::Epsilon()); + tgoal = ogoal; + // End up on the goal. + CreatePointGoalAt(2, 2, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(2)); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(2)); + tgoal = ogoal; + tgoal.x = fixed::FromInt(5)/2; + // End up on the goal. + CreatePointGoalAt(2, 2, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(5)/2); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(2)); + } + + void hierarchical_update_testmap(std::wstring map) + { + CTerrain terrain; + + CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); + sim2.LoadDefaultScripts(); + sim2.ResetState(); + + CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself + + LDR_BeginRegistering(); + mapReader->LoadMap(map, + sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue, + &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL, + &sim2, &sim2.GetSimContext(), -1, false); + LDR_EndRegistering(); + TS_ASSERT_OK(LDR_NonprogressiveLoad()); + + sim2.PreInitGame(); + sim2.InitGame(); + sim2.Update(0); + + CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY); + + pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default"); + HierarchicalPathfinder& hier = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.GetHierarchicalPathfinder(); + + // make copies + const auto pristine_GR = hier.m_GlobalRegions; + const auto pristine_Chunks = hier.m_Chunks; + const HierarchicalPathfinder::EdgesMap pristine_Edges = hier.m_Edges.at(obstructionsMask); + + Grid* pathfinderGrid = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.m_Grid; + + Grid dirtyGrid(hier.m_ChunksW * HierarchicalPathfinder::CHUNK_SIZE,hier.m_ChunksH * HierarchicalPathfinder::CHUNK_SIZE); + srand(1234); + + size_t tries = 20; + for (size_t i = 0; i < tries; ++i) + { + // Dirty a random one + dirtyGrid.reset(); + u8 ci = rand() % (hier.m_ChunksW-10) + 8; + u8 cj = rand() % (hier.m_ChunksH-10) + 8; + dirtyGrid.set(ci * HierarchicalPathfinder::CHUNK_SIZE + 4, cj * HierarchicalPathfinder::CHUNK_SIZE + 4, 1); + + hier.Update(pathfinderGrid, dirtyGrid); + + // Formally speaking we should rather validate that regions exist with the same pixels, but so far + // re-initing regions will keep the same IDs for the same pixels so this is OK. + TS_ASSERT_EQUALS(hier.m_Chunks.at(obstructionsMask), pristine_Chunks.at(obstructionsMask)); + // same here + TS_ASSERT_EQUALS(pristine_Edges, hier.m_Edges.at(obstructionsMask)); + } + } + + void test_hierarchical_update() + { + // This test validates that the "Update" function of the hierarchical pathfinder + // ends up in a correct state (by comparing it with the clean, "Recompute"-d state). + std::vector maps = { L"maps/scenarios/Peloponnese.pmp", L"maps/skirmishes/Corinthian Isthmus (2).pmp", L"maps/skirmishes/Greek Acropolis (2).pmp" }; + +// disable in debug mode, creating the simulation and running the initial turn is too slow and tends to OOM in debug mode. +#ifndef DEBUG + for (std::wstring t : maps) + hierarchical_update_testmap(t); +#endif + } + void test_performance_DISABLED() { CTerrain terrain; @@ -129,6 +286,8 @@ LDR_EndRegistering(); TS_ASSERT_OK(LDR_NonprogressiveLoad()); + sim2.PreInitGame(); + sim2.InitGame(); sim2.Update(0); CmpPtr cmp(sim2, SYSTEM_ENTITY); @@ -158,7 +317,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 +350,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()); } @@ -240,6 +399,8 @@ LDR_EndRegistering(); TS_ASSERT_OK(LDR_NonprogressiveLoad()); + sim2.PreInitGame(); + sim2.InitGame(); sim2.Update(0); std::ofstream stream(OsString("perf2.html").c_str(), std::ofstream::out | std::ofstream::trunc); @@ -295,6 +456,8 @@ LDR_EndRegistering(); TS_ASSERT_OK(LDR_NonprogressiveLoad()); + sim2.PreInitGame(); + sim2.InitGame(); sim2.Update(0); std::ofstream stream(OsString("perf3.html").c_str(), std::ofstream::out | std::ofstream::trunc); @@ -342,7 +505,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 +554,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 @@ -1,4 +1,4 @@ -/* Copyright (C) 2015 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -27,7 +27,7 @@ /** * Hierarchical pathfinder. * - * It doesn't find shortest paths, but deals with connectivity. + * Deals with connectivity (can point A reach point B?). * * The navcell-grid representation of the map is split into fixed-size chunks. * Within a chunk, each maximal set of adjacently-connected passable navcells @@ -35,18 +35,31 @@ * Each region is a vertex in the hierarchical pathfinder's graph. * When two regions in adjacent chunks are connected by passable navcells, * the graph contains an edge between the corresponding two vertexes. - * (There will never be an edge between two regions in the same chunk.) + * By design, there can never be an edge between two regions in the same chunk. + * + * Those fixed-size chunks are used to efficiently compute "global regions" by effectively flood-filling. + * Those can then be used to immediately determine if two reachables points are connected. + * + * The main use of this class is to convert an arbitrary PathGoal to a reachable navcell. + * This happens in MakeGoalReachable. * - * Since regions are typically fairly large, it is possible to determine - * connectivity between any two navcells by mapping them onto their appropriate - * region and then doing a relatively small graph search. */ +#ifdef TEST +class TestCmpPathfinder; +void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal); +#endif + class HierarchicalOverlay; class HierarchicalPathfinder { +#ifdef TEST + friend class TestCmpPathfinder; +#endif public: + typedef u32 GlobalRegionID; + struct RegionID { u8 ci, cj; // chunk ID @@ -54,7 +67,7 @@ RegionID(u8 ci, u8 cj, u16 r) : ci(ci), cj(cj), r(r) { } - bool operator<(RegionID b) const + bool operator<(const RegionID& b) const { // Sort by chunk ID, then by per-chunk region ID if (ci < b.ci) @@ -68,7 +81,7 @@ return r < b.r; } - bool operator==(RegionID b) const + bool operator==(const RegionID& b) const { return ((ci == b.ci) && (cj == b.cj) && (r == b.r)); } @@ -86,30 +99,37 @@ 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; + GlobalRegionID GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const; /** - * Updates @p goal so that it's guaranteed to be reachable from the navcell + * Updates @p goal to a point goal guaranteed to be reachable from the original navcell * @p i0, @p j0 (which is assumed to be on a passable navcell). * - * If the goal is not reachable, it is replaced with a point goal nearest to - * the goal center. + * If the goal is not reachable, it is replaced with an acceptable point goal + * This function does not necessarily return the closest navcell to the goal + * but the one with the lowest f score of the A* algorithm. + * This means it is usually a tradeoff between walking time and distance to the goal. * * 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. + * TODO: since A* is used, it could return the reachable navcell nearest to the penultimate region visited. + * which is probably better (imagine a path that must bend around). + * + * @returns true if the goal was reachable, false otherwise. */ - void MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass); + bool 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. + * Updates @p i, @p j to the nearest passable navcell. + * @param limited: only search a few tiles around (i, j) */ - void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass); + void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass, bool limited = false) 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 { @@ -123,15 +143,15 @@ private: static const u8 CHUNK_SIZE = 96; // number of navcells per side - // TODO PATHFINDER: figure out best number. Probably 64 < n < 128 + // TODO: figure out best number. Probably 64 < n < 128 struct Chunk { u8 m_ChunkI, m_ChunkJ; // chunk ID - u16 m_NumRegions; // number of local region IDs (starting from 1) + std::vector m_RegionsID; // IDs of local region, without 0 u16 m_Regions[CHUNK_SIZE][CHUNK_SIZE]; // local region ID per navcell - cassert(CHUNK_SIZE*CHUNK_SIZE/2 < 65536); // otherwise we could overflow m_NumRegions with a checkerboard pattern + cassert(CHUNK_SIZE*CHUNK_SIZE/2 < 65536); // otherwise we could overflow m_RegionsID with a checkerboard pattern void InitRegions(int ci, int cj, Grid* grid, pass_class_t passClass); @@ -139,39 +159,55 @@ void RegionCenter(u16 r, int& i, int& j) const; + void NearestNavcell(int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const; + void RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const; bool RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const; + +#ifdef TEST + bool operator==(const Chunk& b) const + { + return (m_ChunkI == b.m_ChunkI && m_ChunkJ == b.m_ChunkJ && m_RegionsID == b.m_RegionsID && memcmp(&m_Regions, &b.m_Regions, sizeof(u16) * CHUNK_SIZE * CHUNK_SIZE) == 0); + } +#endif }; typedef std::map > EdgesMap; - void FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges); + void RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges); + void UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges); - void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass); + void UpdateGlobalRegions(const std::map>& needNewGlobalRegionMap); - void FindPassableRegions(std::set& regions, pass_class_t passClass); + template + void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const; + + void FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, 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; + u8 m_ChunksW, m_ChunksH; std::map > m_Chunks; std::map m_Edges; + std::map> m_GlobalRegions; + GlobalRegionID m_NextGlobalRegionID; + // Passability classes for which grids will be updated when calling Update std::map m_PassClassMasks; Index: source/simulation2/helpers/HierarchicalPathfinder.cpp =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.cpp +++ source/simulation2/helpers/HierarchicalPathfinder.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2016 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -22,6 +22,8 @@ #include "graphics/Overlay.h" #include "ps/Profile.h" +#include "Geometry.h" + // Find the root ID of a region, used by InitRegions inline u16 RootID(u16 x, const std::vector& v) { @@ -39,6 +41,8 @@ memset(m_Regions, 0, sizeof(m_Regions)); + m_RegionsID.clear(); + int i0 = ci * CHUNK_SIZE; int j0 = cj * CHUNK_SIZE; int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W); @@ -110,14 +114,13 @@ } } - // Directly point the root ID - m_NumRegions = 0; + // Mark connected regions as being the same ID (i.e. the lowest) for (u16 i = 1; i < regionID+1; ++i) { - if (connect[i] == i) - ++m_NumRegions; - else + if (connect[i] != i) connect[i] = RootID(i, connect); + else + m_RegionsID.push_back(connect[i]); } // Replace IDs by the root ID @@ -173,6 +176,53 @@ /** * Returns the global navcell coords, and the squared distance to the goal + * navcell, of whichever navcell inside the given chunk is closest to + * that goal. + */ +void HierarchicalPathfinder::Chunk::NearestNavcell(int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const +{ + iBest = 0; + jBest = 0; + dist2Best = std::numeric_limits::max(); + u32 rowBest = std::numeric_limits::max(); + + // TODO: it might be faster to try the best navcell first, since we can compute that. + for (int j = 0; j < CHUNK_SIZE; ++j) + { + for (int i = 0; i < CHUNK_SIZE; ++i) + { + if (m_Regions[j][i] == 0) + continue; + + /** + * Line-point distance. Over a single row, dist2 is first monotonically decreasing, then monotonically increasing + * Thus once we stop improving, we can safely break. + * However, we cannot compare across lines because obstructed chunks may skew numbers. + * Imagine a situation like this (G is the goal, X a passable cell, - an impassable one): + * ----G- + * -X---- + * X---X- + * Despite the first X on the third row being farther, the third row has a better navcell. + */ + u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - iGoal)*(i + m_ChunkI*CHUNK_SIZE - iGoal) + + (j + m_ChunkJ*CHUNK_SIZE - jGoal)*(j + m_ChunkJ*CHUNK_SIZE - jGoal); + if (dist2 < dist2Best) + { + iBest = i + m_ChunkI*CHUNK_SIZE; + jBest = j + m_ChunkJ*CHUNK_SIZE; + dist2Best = dist2; + rowBest = dist2; + } + else if (dist2 < rowBest) + rowBest = dist2; + else + break; + } + } +} + +/** + * Returns the global navcell coords, and the squared distance to the goal * navcell, of whichever navcell inside the given region is closest to * that goal. */ @@ -191,7 +241,6 @@ u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - iGoal)*(i + m_ChunkI*CHUNK_SIZE - iGoal) + (j + m_ChunkJ*CHUNK_SIZE - jGoal)*(j + m_ChunkJ*CHUNK_SIZE - jGoal); - if (dist2 < dist2Best) { iBest = i + m_ChunkI*CHUNK_SIZE; @@ -361,15 +410,18 @@ m_W = grid->m_W; m_H = grid->m_H; + ENSURE((grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE < 256 && (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE < 256); // else the u8 Chunk::m_ChunkI will overflow + // Divide grid into chunks with round-to-positive-infinity m_ChunksW = (grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE; m_ChunksH = (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE; - ENSURE(m_ChunksW < 256 && m_ChunksH < 256); // else the u8 Chunk::m_ChunkI will overflow - m_Chunks.clear(); m_Edges.clear(); + // reset global regions + m_NextGlobalRegionID = 1; + for (auto& passClassMask : allPassClasses) { pass_class_t passClass = passClassMask.second; @@ -385,16 +437,32 @@ } // Construct the search graph over the regions - EdgesMap& edges = m_Edges[passClass]; + RecomputeAllEdges(passClass, edges); - for (int cj = 0; cj < m_ChunksH; ++cj) - { - for (int ci = 0; ci < m_ChunksW; ++ci) - { - FindEdges(ci, cj, passClass, edges); - } - } + // Spread global regions. + std::map& globalRegion = m_GlobalRegions[passClass]; + globalRegion.clear(); + for (u8 cj = 0; cj < m_ChunksH; ++cj) + for (u8 ci = 0; ci < m_ChunksW; ++ci) + for (u16 rid : GetChunk(ci, cj, passClass).m_RegionsID) + { + RegionID reg{ci,cj,rid}; + if (globalRegion.find(reg) == globalRegion.end()) + { + GlobalRegionID ID = m_NextGlobalRegionID++; + + globalRegion.insert({ reg, ID }); + // avoid creating an empty link if possible, FindReachableRegions uses [] which calls the default constructor + if (edges.find(reg) != edges.end()) + { + std::set reachable; + FindReachableRegions(reg, reachable, passClass); + for (const RegionID& region : reachable) + globalRegion.insert({ region, ID }); + } + } + } } if (m_DebugOverlay) @@ -409,11 +477,27 @@ { PROFILE3("Hierarchical Update"); - for (int cj = 0; cj < m_ChunksH; ++cj) + ASSERT(m_NextGlobalRegionID < std::numeric_limits::max()); + + std::map> needNewGlobalRegionMap; + + // Algorithm for the partial update: + // 1. Loop over chunks. + // 2. For any dirty chunk: + // - remove all regions from the global region map + // - remove all edges, by removing the neighbor connection with them and then deleting us + // - recreate regions inside the chunk + // - reconnect the regions. We may do too much work if we reconnect with a dirty chunk, but that's fine. + // 3. Recreate global regions. + // This means that if any chunk changes, we may need to flood (at most once) the whole map. + // That's quite annoying, but I can't think of an easy way around it. + // If we could be sure that a region's topology hasn't changed, we could skip removing its global region + // but that's non trivial as we have no easy way to determine said topology (regions could "switch" IDs on update for now). + for (u8 cj = 0; cj < m_ChunksH; ++cj) { int j0 = cj * CHUNK_SIZE; int j1 = std::min(j0 + CHUNK_SIZE, (int)dirtinessGrid.m_H); - for (int ci = 0; ci < m_ChunksW; ++ci) + for (u8 ci = 0; ci < m_ChunksW; ++ci) { // Skip chunks where no navcells are dirty. int i0 = ci * CHUNK_SIZE; @@ -425,27 +509,35 @@ { pass_class_t passClass = passClassMask.second; Chunk& a = m_Chunks[passClass].at(ci + cj*m_ChunksW); + + // Clean up edges and global region ID + EdgesMap& edgeMap = m_Edges[passClass]; + for (u16 i : a.m_RegionsID) + { + RegionID reg{ci, cj, i}; + m_GlobalRegions[passClass].erase(reg); + for (const RegionID& neighbor : edgeMap[reg]) + { + edgeMap[neighbor].erase(reg); + if (edgeMap[neighbor].empty()) + edgeMap.erase(neighbor); + } + edgeMap.erase(reg); + } + + // recompute local regions (not global regions) a.InitRegions(ci, cj, grid, passClass); - } - } - } - // TODO: Also be clever with edges - m_Edges.clear(); - for (const std::pair& passClassMask : m_PassClassMasks) - { - pass_class_t passClass = passClassMask.second; - EdgesMap& edges = m_Edges[passClass]; + for (u16 i : a.m_RegionsID) + needNewGlobalRegionMap[passClass].push_back(RegionID{ci, cj, i}); - for (int cj = 0; cj < m_ChunksH; ++cj) - { - for (int ci = 0; ci < m_ChunksW; ++ci) - { - FindEdges(ci, cj, passClass, edges); + UpdateEdges(ci, cj, passClass, edgeMap); } } } + UpdateGlobalRegions(needNewGlobalRegionMap); + if (m_DebugOverlay) { PROFILE("debug overlay"); @@ -455,22 +547,15 @@ } /** - * Find edges between regions in this chunk and the adjacent below/left chunks. + * Connect a chunk's regions to their neighbors. Not optimised for global recomputing. + * TODO: reduce code duplication with below */ -void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) +void HierarchicalPathfinder::UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) { std::vector& chunks = m_Chunks[passClass]; 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 - // (i.e. are passable navcells) then add a graph edge between those regions. - // (We don't need to test for duplicates since EdgesMap already uses a - // std::set which will drop duplicate entries.) - // But as set.insert can be quite slow on large collection, and that we usually - // try to insert the same values, we cache the previous one for a fast test. - if (ci > 0) { Chunk& b = chunks.at(cj*m_ChunksW + (ci-1)); @@ -492,6 +577,27 @@ } } + if (ci < m_ChunksW-1) + { + 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) + { + RegionID ra = a.Get(CHUNK_SIZE-1, j); + RegionID rb = b.Get(0, j); + if (ra.r && rb.r) + { + if (ra == raPrev && rb == rbPrev) + continue; + edges[ra].insert(rb); + edges[rb].insert(ra); + raPrev = ra; + rbPrev = rb; + } + } + } + if (cj > 0) { Chunk& b = chunks.at((cj-1)*m_ChunksW + ci); @@ -513,6 +619,94 @@ } } + if (cj < m_ChunksH - 1) + { + 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) + { + RegionID ra = a.Get(i, CHUNK_SIZE-1); + RegionID rb = b.Get(i, 0); + if (ra.r && rb.r) + { + if (ra == raPrev && rb == rbPrev) + continue; + edges[ra].insert(rb); + edges[rb].insert(ra); + raPrev = ra; + rbPrev = rb; + } + } + } +} + +/** + * Find edges between regions in all chunks, in an optimised manner (only look at top/left) + */ +void HierarchicalPathfinder::RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges) +{ + std::vector& chunks = m_Chunks[passClass]; + + edges.clear(); + + for (int cj = 0; cj < m_ChunksH; ++cj) + { + for (int ci = 0; ci < m_ChunksW; ++ci) + { + 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 + // (i.e. are passable navcells) then add a graph edge between those regions. + // (We don't need to test for duplicates since EdgesMap already uses a + // std::set which will drop duplicate entries.) + // But as set.insert can be quite slow on large collection, and that we usually + // try to insert the same values, we cache the previous one for a fast test. + + if (ci > 0) + { + 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) + { + RegionID ra = a.Get(0, j); + RegionID rb = b.Get(CHUNK_SIZE-1, j); + if (ra.r && rb.r) + { + if (ra == raPrev && rb == rbPrev) + continue; + edges[ra].insert(rb); + edges[rb].insert(ra); + raPrev = ra; + rbPrev = rb; + } + } + } + + if (cj > 0) + { + 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) + { + RegionID ra = a.Get(i, 0); + RegionID rb = b.Get(i, CHUNK_SIZE-1); + if (ra.r && rb.r) + { + if (ra == raPrev && rb == rbPrev) + continue; + edges[ra].insert(rb); + edges[rb].insert(ra); + raPrev = ra; + rbPrev = rb; + } + } + } + } + } } /** @@ -550,97 +744,321 @@ xz.push_back(b.Y.ToFloat()); m_DebugOverlayLines.emplace_back(); - m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0); + m_DebugOverlayLines.back().m_Color = CColor(1.0, 0.0, 0.0, 1.0); SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true); } } } -HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) +void HierarchicalPathfinder::UpdateGlobalRegions(const std::map>& needNewGlobalRegionMap) +{ + // Use FindReachableRegions because we cannot be sure, even if we find a non-dirty chunk nearby, + // that we weren't the only bridge connecting that chunk to the rest of the global region. + for (const std::pair>& regionsInNeed : needNewGlobalRegionMap) + for (const RegionID& reg : regionsInNeed.second) + { + std::map& globalRegions = m_GlobalRegions[regionsInNeed.first]; + // if we have already been given a region, skip us. + if (globalRegions.find(reg) != globalRegions.end()) + continue; + + std::set reachable; + FindReachableRegions(reg, reachable, regionsInNeed.first); + + GlobalRegionID ID = m_NextGlobalRegionID++; + + for (const RegionID& reg : reachable) + globalRegions[reg] = ID; + } +} + +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) +HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const +{ + RegionID region = Get(i, j, passClass); + if (region.r == 0) + return (GlobalRegionID)0; + return m_GlobalRegions.at(passClass).at(region); +} + +void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal) +{ + // recreate the goal as a point-goal. + PathGoal newGoal; + newGoal.type = PathGoal::POINT; + Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z); + + CFixedVector2D relativePos(goal.x - newGoal.x, goal.z - newGoal.z); + CFixedVector2D u(fixed::FromInt(1), fixed::FromInt(0)); + CFixedVector2D v(fixed::FromInt(0), fixed::FromInt(1)); + CFixedVector2D halfsize(fixed::FromInt(1)/2, fixed::FromInt(1)/2); + + CFixedVector2D bottomRight = CFixedVector2D(newGoal.x, newGoal.z) + halfsize; + + // Try to get close to the goal. If the goal center is in the navcell, pick that, else choose an edge. + if (Geometry::PointIsInSquare(relativePos, u, v, halfsize)) + { + newGoal.x = goal.x; + newGoal.z = goal.z; + } + else + { + relativePos = Geometry::NearestPointOnSquare(relativePos, u, v, halfsize); + newGoal.x += relativePos.X; + newGoal.z += relativePos.Y; + } + // We must not change the navcell we're on, and this could (since we could be on the bottom/right edge). + // NavcellContainsGoal returns true only for the left/upper edges, + // so we can just epsilon to the left/up. + if (newGoal.x >= bottomRight.X) + newGoal.x -= fixed::Epsilon(); + if (newGoal.z >= bottomRight.Y) + newGoal.z -= fixed::Epsilon(); + + goal = newGoal; +} + +bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const { PROFILE2("MakeGoalReachable"); - RegionID source = Get(i0, j0, passClass); - // Find everywhere that's reachable - std::set reachableRegions; - FindReachableRegions(source, reachableRegions, passClass); + // Make the goal reachable. + // In case the goal is a passable navcell, we'll just compare global regions and return immediately. + // If the goal isn't a passable navcell, we'll try to make it one using the limited FindNearestPassableNavcell + // This optimises for a common case where we want to go to a tree and similar things. + // If the goal is decidedly not passable, we'll revert to a flood-fill. + + //////////////////////////////// + // determine if we will be able to reach the goal. + u16 gi, gj; + Pathfinding::NearestNavcell(goal.x, goal.z, gi, gj, m_W, m_H); + + std::set goalRegions; + FindGoalRegions(gi, gj, goal, goalRegions, passClass); + + std::vector targetRegions; + + GlobalRegionID startID = GetGlobalRegion(i0, j0, passClass); + bool reachable = false; + for (const RegionID& r : goalRegions) + if (m_GlobalRegions.at(passClass).at(r) == startID) + { + reachable = true; + targetRegions.push_back(r); + } + //////////////////////////////// + // if the goal is a point-goal and reachable, just return early. + if (reachable && goal.type == PathGoal::POINT) + { + CreatePointGoalAt(gi, gj, goal); + return reachable; + } - // Check whether any reachable region contains the goal - // And get the navcell that's the closest to the point + //////////////////////////////// + // If the goal is unreachable, we'd still like to get at least somewhere on the closest reachable region. + // Use a limited FindNearestPassableNavcell search, for the 5 neighboring navcells (common case: a tree…) + // if that returns a passable, reachable navcell, go there instead. + if (!reachable) + { + u16 ngi = gi, ngj = gj; + FindNearestPassableNavcell(ngi, ngj, passClass, true); - u16 bestI = 0; - u16 bestJ = 0; - u32 dist2Best = std::numeric_limits::max(); + if (GetGlobalRegion(ngi, ngj, passClass) == startID) + { + gi = ngi; + gj = ngj; + CreatePointGoalAt(gi, gj, goal); + return reachable; + } + } - for (const RegionID& region : reachableRegions) + //////////////////////////////// + // Loop through regions and get the best navcell. + u16 bestI = 0, bestJ = 0; + u32 bestDist = std::numeric_limits::max(); + if (reachable) + { + // If we have target regions, we can just check them individually, which is reasonably fast. + for (const RegionID& region : targetRegions) + { + u16 ri, rj; + u32 dist; + GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, ri, rj, dist); + if (dist < bestDist) + { + bestI = ri; + bestJ = rj; + bestDist = dist; + } + } + } + else { - // Skip region if its chunk doesn't contain the goal area - entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (region.ci * CHUNK_SIZE); - entity_pos_t z0 = Pathfinding::NAVCELL_SIZE * (region.cj * CHUNK_SIZE); - entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE; - entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE; - if (!goal.RectContainsGoal(x0, z0, x1, z1)) - continue; + // Otherwise we should try all reachable regions. + // To avoid doing too much work, we'll sort them by distance from chunk center + // If we've already found a point from a region and our current chunk center is at least √2*region_width farther + // we can be sure any navcell in this region will not be better, so we can exit. - u16 i,j; - u32 dist2; + struct RegionComparator + { + RegionComparator() = delete; + RegionComparator(u16 i, u16 j): gi (i), gj(j) {}; - // If the region contains the goal area, the goal is reachable - // Remember the best point for optimization. - if (GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, i, j, dist2)) - { - // If it's a point, no need to move it, we're done - if (goal.type == PathGoal::POINT) - return; - if (dist2 < dist2Best) + u32 dist(const HierarchicalPathfinder::RegionID& region) const { - bestI = i; - bestJ = j; - dist2Best = dist2; + return (region.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) * (region.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) + + (region.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj) * (region.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj); + } + bool operator()(const HierarchicalPathfinder::RegionID& a, const HierarchicalPathfinder::RegionID& b) const + { + if (dist(a) < dist(b)) + return true; + if (dist(a) > dist(b)) + return false; + return a.r < b.r; + } + u16 gi, gj; + }; + + // TODO: if this becomes too slow, perhaps roll-out an optimised insert-sort here. + RegionComparator comparator(gi, gj); + std::set reachableRegions(comparator); + FindReachableRegions(Get(i0, j0, passClass), reachableRegions, passClass); + + u32 bestChunkCenterDist = std::numeric_limits::max(); + u32 maxDistFromChunkCenter = (fixed::FromInt(3) / 2 * HierarchicalPathfinder::CHUNK_SIZE).ToInt_RoundToInfinity(); + ENSURE(maxDistFromChunkCenter < std::numeric_limits::max()); + maxDistFromChunkCenter *= maxDistFromChunkCenter; + for (const RegionID& region : reachableRegions) + { + u32 chunkDist = comparator.dist(region); + if (bestDist < std::numeric_limits::max() && (ssize_t)chunkDist > (ssize_t)bestChunkCenterDist + maxDistFromChunkCenter) + break; // break instead of continue as the set is ordered in increased distance. + + int ri, rj; + u32 dist; + GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, gi, gj, ri, rj, dist); + if (dist < bestDist) + { + bestI = ri; + bestJ = rj; + bestDist = dist; + bestChunkCenterDist = chunkDist; } } } + CreatePointGoalAt(bestI, bestJ, goal); + + return reachable; +} - // If the goal area wasn't reachable, - // find the navcell that's nearest to the goal's center - if (dist2Best == std::numeric_limits::max()) - { - u16 iGoal, jGoal; - Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H); - - FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass); - - // Construct a new point goal at the nearest reachable navcell - PathGoal newGoal; - newGoal.type = PathGoal::POINT; - Pathfinding::NavcellCenter(iGoal, jGoal, newGoal.x, newGoal.z); - goal = newGoal; +template +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 + reachable.insert(from); + + const EdgesMap& edgeMap = m_Edges.at(passClass); + if (edgeMap.find(from) == edgeMap.end()) return; - } - ENSURE(dist2Best != std::numeric_limits::max()); - PathGoal newGoal; - newGoal.type = PathGoal::POINT; - Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z); - goal = newGoal; + std::vector open; + open.reserve(64); + open.push_back(from); + + while (!open.empty()) + { + RegionID curr = open.back(); + open.pop_back(); + for (const RegionID& region : edgeMap.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) + open.push_back(region); + } } -void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) +void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass, bool limited) const { - std::set regions; - FindPassableRegions(regions, passClass); - FindNearestNavcellInRegions(regions, i, j, passClass); + // If our current region is not 0, then this is passable, return immediately. + if (Get(i, j, passClass).r != 0) + return; + + u32 dist2Best = std::numeric_limits::max(); + + u16 oi = i, oj = j; + // in most cases we're < 5 cells away so go for this first + for (u16 tj = std::max(0, oj-5); tj <= std::min(m_H-1, oj+5); ++tj) + for (u16 ti = std::max(0, oi-5); ti <= std::min(m_W-1, oi+5); ++ti) + if (Get(ti, tj, passClass).r != 0) + { + u32 dist = (ti-oi)*(ti-oi) + (tj-oj)*(tj-oj); + if (dist < dist2Best) + { + i = ti; + j = tj; + dist2Best = dist; + } + } + if (dist2Best < std::numeric_limits::max() || limited) + return; + + // We are on an impassable area, so we cannot rely on our accessibility. + // This function does not necessarily need to return the absolute closest navcell, + // since standing on impassable terrain is already a little wonky + // We'll expand in a cross-wise way, returning the best passable cell after each expansion (if any) + // this will ensure that we return an acceptable close navcell in general. + // since the first time we could be close to an edge, we'll expand at least once. + + u16 iBest = i; + u16 jBest = j; + dist2Best = std::numeric_limits::max(); + + u16 si = i/CHUNK_SIZE, sj = j/CHUNK_SIZE; + std::set> visited = { { si, sj } }; + std::vector> openList = { { si, sj } }, openListTemp; + static const int expander[4][2] = { { 1, 0 },{ -1, 0 },{ 0, 1 },{ 0, -1 } }; + for (size_t step = 0;;++step) + { + openListTemp.clear(); + for (std::pair chunk : openList) + { + // go through all regions of a chunk + int tempi, tempj; + u32 dist2; + GetChunk(chunk.first, chunk.second, passClass).NearestNavcell(i, j, tempi, tempj, dist2); + if (dist2 < dist2Best) + { + iBest = tempi; + jBest = tempj; + dist2Best = dist2; + } + // expand cross + for (const int* dir : expander) + { + u8 x = (u8)std::min(std::max((int)si + dir[0], 0), (int)(m_ChunksW-1)); + u8 z = (u8)std::min(std::max((int)sj + dir[1], 0), (int)(m_ChunksH-1)); + if (visited.insert({ x, z }).second) + openListTemp.push_back({ x, z }); + } + } + if (openListTemp.empty() || (step > 0 && dist2Best < std::numeric_limits::max())) + break; + openList.swap(openListTemp); + } + i = iBest; + j = jBest; } -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,47 +1115,46 @@ jGoal = jBest; } -void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) +void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const { - // Flood-fill the region graph, starting at 'from', - // collecting all the regions that are reachable via edges - - std::vector open; - open.push_back(from); - reachable.insert(from); - - while (!open.empty()) + if (goal.type == PathGoal::POINT) { - RegionID curr = open.back(); - open.pop_back(); - - for (const RegionID& region : m_Edges[passClass][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) - open.push_back(region); + RegionID region = Get(gi, gj, passClass); + if (region.r > 0) + regions.insert(region); + return; } -} -void HierarchicalPathfinder::FindPassableRegions(std::set& regions, pass_class_t passClass) -{ - // Construct a set of all regions of all chunks for this pass class - for (const Chunk& chunk : m_Chunks[passClass]) - { - // region 0 is impassable tiles - for (int r = 1; r <= chunk.m_NumRegions; ++r) - regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r)); - } + // For non-point cases, we'll test each region inside the bounds of the goal. + // we might occasionally test a few too many for circles but it's not too bad. + // Note that this also works in the Inverse-circle / Inverse-square case + // Since our ranges are inclusive, we will necessarily test at least the perimeter/outer bound of the goal. + // If we find a navcell, great, if not, well then we'll be surrounded by an impassable barrier. + // Since in the Inverse-XX case we're supposed to start inside, then we can't ever reach the goal so it's good enough. + // It's not worth it to skip the "inner" regions since we'd need ranges above CHUNK_SIZE for that to start mattering + // (and even then not always) and that just doesn't happen for Inverse-XX goals + int size = (std::max(goal.hh, goal.hw) * 3 / 2).ToInt_RoundToInfinity(); + + u16 a,b; u32 c; // unused params for RegionNearestNavcellInGoal + + for (u8 sz = std::max(0,(gj - size) / CHUNK_SIZE); sz <= std::min(m_ChunksH-1, (gj + size + 1) / CHUNK_SIZE); ++sz) + for (u8 sx = std::max(0,(gi - size) / CHUNK_SIZE); sx <= std::min(m_ChunksW-1, (gi + size + 1) / CHUNK_SIZE); ++sx) + { + const Chunk& chunk = GetChunk(sx, sz, passClass); + for (u16 i : chunk.m_RegionsID) + if (chunk.RegionNearestNavcellInGoal(i, 0, 0, goal, a, b, c)) + regions.insert(RegionID{sx, sz, i}); + } } -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 +1162,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 @@ -1,4 +1,4 @@ -/* Copyright (C) 2017 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -18,6 +18,7 @@ #ifndef INCLUDED_LONGPATHFINDER #define INCLUDED_LONGPATHFINDER +#include #include "Pathfinding.h" #include "HierarchicalPathfinder.h" @@ -162,6 +163,10 @@ LongPathfinder(); ~LongPathfinder(); +#ifdef TEST + HierarchicalPathfinder& GetHierarchicalPathfinder() { return m_PathfinderHier; } +#endif + void SetDebugOverlay(bool enabled); void SetHierDebugOverlay(bool enabled, const CSimContext *simContext) @@ -217,7 +222,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) { @@ -237,6 +242,16 @@ void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, std::vector excludedRegions, WaypointPath& path); + bool MakeGoalReachable(u16 i0, u16 j0, PathGoal &goal, pass_class_t passClass) { return m_PathfinderHier.MakeGoalReachable(i0, j0, goal, passClass); }; + + void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) { return m_PathfinderHier.FindNearestPassableNavcell(i, j, passClass); }; + + bool NavcellIsReachable(u16 i0, u16 j0, u16 i1, u16 j1, pass_class_t passClass) + { + return m_PathfinderHier.GetGlobalRegion(i0, j0, passClass) == m_PathfinderHier.GetGlobalRegion(i1, j1, passClass); + }; + + Grid GetConnectivityGrid(pass_class_t passClass) { return m_PathfinderHier.GetConnectivityGrid(passClass); @@ -251,33 +266,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 +309,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 @@ -1,4 +1,4 @@ -/* Copyright (C) 2017 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -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,14 @@ } } -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); + PROFILE3("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 +727,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); @@ -896,16 +897,18 @@ ImprovePathWaypoints(path, passClass, origGoal.maxdist, x0, z0); + SAFE_DELETE(state.tiles); + // 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 +962,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 +983,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/PathGoal.cpp =================================================================== --- source/simulation2/helpers/PathGoal.cpp +++ source/simulation2/helpers/PathGoal.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2018 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -27,13 +27,13 @@ static bool NavcellContainsCircle(int i, int j, fixed x, fixed z, fixed r, bool inside) { // Accept any navcell (i,j) that contains a point which is inside[/outside] - // (or on the edge of) the circle + // (or on the left/upper edge of) the circle. + // The reason for picking only two edges is that an edge should only belong to one navcell. - // Get world-space bounds of navcell entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); - entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; - entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE - fixed::Epsilon(); + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE - fixed::Epsilon(); if (inside) { @@ -62,13 +62,13 @@ bool inside) { // Accept any navcell (i,j) that contains a point which is inside[/outside] - // (or on the edge of) the square + // (or on the left/upper edge of) the square. + // The reason for picking only two edges is that an edge should only belong to one navcell. - // Get world-space bounds of navcell entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); - entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; - entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE - fixed::Epsilon(); + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE - fixed::Epsilon(); if (inside) { Index: source/simulation2/helpers/Render.cpp =================================================================== --- source/simulation2/helpers/Render.cpp +++ source/simulation2/helpers/Render.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2017 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -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);