Index: ps/trunk/source/simulation2/components/CCmpAIManager.cpp =================================================================== --- ps/trunk/source/simulation2/components/CCmpAIManager.cpp (revision 22277) +++ ps/trunk/source/simulation2/components/CCmpAIManager.cpp (revision 22278) @@ -1,1189 +1,1199 @@ /* Copyright (C) 2018 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #include "precompiled.h" #include "simulation2/system/Component.h" #include "ICmpAIManager.h" #include "simulation2/MessageTypes.h" #include "graphics/Terrain.h" #include "lib/timer.h" #include "lib/tex/tex.h" #include "lib/allocators/shared_ptr.h" #include "ps/CLogger.h" #include "ps/Filesystem.h" #include "ps/Profile.h" #include "ps/scripting/JSInterface_VFS.h" #include "ps/TemplateLoader.h" #include "ps/Util.h" #include "simulation2/components/ICmpAIInterface.h" #include "simulation2/components/ICmpCommandQueue.h" #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/components/ICmpRangeManager.h" #include "simulation2/components/ICmpTemplateManager.h" #include "simulation2/components/ICmpTerritoryManager.h" +#include "simulation2/helpers/HierarchicalPathfinder.h" #include "simulation2/helpers/LongPathfinder.h" #include "simulation2/serialization/DebugSerializer.h" #include "simulation2/serialization/StdDeserializer.h" #include "simulation2/serialization/StdSerializer.h" #include "simulation2/serialization/SerializeTemplates.h" extern void QuitEngine(); /** * @file * Player AI interface. * AI is primarily scripted, and the CCmpAIManager component defined here * takes care of managing all the scripts. * * To avoid slow AI scripts causing jerky rendering, they are run in a background * thread (maintained by CAIWorker) so that it's okay if they take a whole simulation * turn before returning their results (though preferably they shouldn't use nearly * that much CPU). * * CCmpAIManager grabs the world state after each turn (making use of AIInterface.js * and AIProxy.js to decide what data to include) then passes it to CAIWorker. * The AI scripts will then run asynchronously and return a list of commands to execute. * Any attempts to read the command list (including indirectly via serialization) * will block until it's actually completed, so the rest of the engine should avoid * reading it for as long as possible. * * JS::Values are passed between the game and AI threads using ScriptInterface::StructuredClone. * * TODO: actually the thread isn't implemented yet, because performance hasn't been * sufficiently problematic to justify the complexity yet, but the CAIWorker interface * is designed to hopefully support threading when we want it. */ /** * Implements worker thread for CCmpAIManager. */ class CAIWorker { private: class CAIPlayer { NONCOPYABLE(CAIPlayer); public: CAIPlayer(CAIWorker& worker, const std::wstring& aiName, player_id_t player, u8 difficulty, const std::wstring& behavior, shared_ptr scriptInterface) : m_Worker(worker), m_AIName(aiName), m_Player(player), m_Difficulty(difficulty), m_Behavior(behavior), m_ScriptInterface(scriptInterface), m_Obj(scriptInterface->GetJSRuntime()) { } bool Initialise() { // LoadScripts will only load each script once even though we call it for each player if (!m_Worker.LoadScripts(m_AIName)) return false; JSContext* cx = m_ScriptInterface->GetContext(); JSAutoRequest rq(cx); OsPath path = L"simulation/ai/" + m_AIName + L"/data.json"; JS::RootedValue metadata(cx); m_Worker.LoadMetadata(path, &metadata); if (metadata.isUndefined()) { LOGERROR("Failed to create AI player: can't find %s", path.string8()); return false; } // Get the constructor name from the metadata std::string moduleName; std::string constructor; JS::RootedValue objectWithConstructor(cx); // object that should contain the constructor function JS::RootedValue global(cx, m_ScriptInterface->GetGlobalObject()); JS::RootedValue ctor(cx); if (!m_ScriptInterface->HasProperty(metadata, "moduleName")) { LOGERROR("Failed to create AI player: %s: missing 'moduleName'", path.string8()); return false; } m_ScriptInterface->GetProperty(metadata, "moduleName", moduleName); if (!m_ScriptInterface->GetProperty(global, moduleName.c_str(), &objectWithConstructor) || objectWithConstructor.isUndefined()) { LOGERROR("Failed to create AI player: %s: can't find the module that should contain the constructor: '%s'", path.string8(), moduleName); return false; } if (!m_ScriptInterface->GetProperty(metadata, "constructor", constructor)) { LOGERROR("Failed to create AI player: %s: missing 'constructor'", path.string8()); return false; } // Get the constructor function from the loaded scripts if (!m_ScriptInterface->GetProperty(objectWithConstructor, constructor.c_str(), &ctor) || ctor.isNull()) { LOGERROR("Failed to create AI player: %s: can't find constructor '%s'", path.string8(), constructor); return false; } m_ScriptInterface->GetProperty(metadata, "useShared", m_UseSharedComponent); // Set up the data to pass as the constructor argument JS::RootedValue settings(cx); m_ScriptInterface->Eval(L"({})", &settings); m_ScriptInterface->SetProperty(settings, "player", m_Player, false); m_ScriptInterface->SetProperty(settings, "difficulty", m_Difficulty, false); m_ScriptInterface->SetProperty(settings, "behavior", m_Behavior, false); if (!m_UseSharedComponent) { ENSURE(m_Worker.m_HasLoadedEntityTemplates); m_ScriptInterface->SetProperty(settings, "templates", m_Worker.m_EntityTemplates, false); } JS::AutoValueVector argv(cx); argv.append(settings.get()); m_ScriptInterface->CallConstructor(ctor, argv, &m_Obj); if (m_Obj.get().isNull()) { LOGERROR("Failed to create AI player: %s: error calling constructor '%s'", path.string8(), constructor); return false; } return true; } void Run(JS::HandleValue state, int playerID) { m_Commands.clear(); m_ScriptInterface->CallFunctionVoid(m_Obj, "HandleMessage", state, playerID); } // overloaded with a sharedAI part. // javascript can handle both natively on the same function. void Run(JS::HandleValue state, int playerID, JS::HandleValue SharedAI) { m_Commands.clear(); m_ScriptInterface->CallFunctionVoid(m_Obj, "HandleMessage", state, playerID, SharedAI); } void InitAI(JS::HandleValue state, JS::HandleValue SharedAI) { m_Commands.clear(); m_ScriptInterface->CallFunctionVoid(m_Obj, "Init", state, m_Player, SharedAI); } CAIWorker& m_Worker; std::wstring m_AIName; player_id_t m_Player; u8 m_Difficulty; std::wstring m_Behavior; bool m_UseSharedComponent; // Take care to keep this declaration before heap rooted members. Destructors of heap rooted // members have to be called before the runtime destructor. shared_ptr m_ScriptInterface; JS::PersistentRootedValue m_Obj; std::vector > m_Commands; }; public: struct SCommandSets { player_id_t player; std::vector > commands; }; CAIWorker() : m_ScriptInterface(new ScriptInterface("Engine", "AI", g_ScriptRuntime)), m_TurnNum(0), m_CommandsComputed(true), m_HasLoadedEntityTemplates(false), m_HasSharedComponent(false), m_SerializablePrototypes(new ObjectIdCache(g_ScriptRuntime)), m_EntityTemplates(g_ScriptRuntime->m_rt), m_SharedAIObj(g_ScriptRuntime->m_rt), m_PassabilityMapVal(g_ScriptRuntime->m_rt), m_TerritoryMapVal(g_ScriptRuntime->m_rt) { m_ScriptInterface->ReplaceNondeterministicRNG(m_RNG); m_ScriptInterface->SetCallbackData(static_cast (this)); m_SerializablePrototypes->init(); JS_AddExtraGCRootsTracer(m_ScriptInterface->GetJSRuntime(), Trace, this); m_ScriptInterface->RegisterFunction("PostCommand"); m_ScriptInterface->RegisterFunction("IncludeModule"); m_ScriptInterface->RegisterFunction("Exit"); m_ScriptInterface->RegisterFunction("ComputePath"); m_ScriptInterface->RegisterFunction, u32, u32, u32, CAIWorker::DumpImage>("DumpImage"); m_ScriptInterface->RegisterFunction("GetTemplate"); JSI_VFS::RegisterScriptFunctions_Simulation(*(m_ScriptInterface.get())); // Globalscripts may use VFS script functions m_ScriptInterface->LoadGlobalScripts(); } ~CAIWorker() { JS_RemoveExtraGCRootsTracer(m_ScriptInterface->GetJSRuntime(), Trace, this); } bool HasLoadedEntityTemplates() const { return m_HasLoadedEntityTemplates; } bool LoadScripts(const std::wstring& moduleName) { // Ignore modules that are already loaded if (m_LoadedModules.find(moduleName) != m_LoadedModules.end()) return true; // Mark this as loaded, to prevent it recursively loading itself m_LoadedModules.insert(moduleName); // Load and execute *.js VfsPaths pathnames; if (vfs::GetPathnames(g_VFS, L"simulation/ai/" + moduleName + L"/", L"*.js", pathnames) < 0) { LOGERROR("Failed to load AI scripts for module %s", utf8_from_wstring(moduleName)); return false; } for (const VfsPath& path : pathnames) { if (!m_ScriptInterface->LoadGlobalScriptFile(path)) { LOGERROR("Failed to load script %s", path.string8()); return false; } } return true; } static void IncludeModule(ScriptInterface::CxPrivate* pCxPrivate, const std::wstring& name) { ENSURE(pCxPrivate->pCBData); CAIWorker* self = static_cast (pCxPrivate->pCBData); self->LoadScripts(name); } static void PostCommand(ScriptInterface::CxPrivate* pCxPrivate, int playerid, JS::HandleValue cmd) { ENSURE(pCxPrivate->pCBData); CAIWorker* self = static_cast (pCxPrivate->pCBData); self->PostCommand(playerid, cmd); } void PostCommand(int playerid, JS::HandleValue cmd) { for (size_t i=0; im_Player == playerid) { m_Players[i]->m_Commands.push_back(m_ScriptInterface->WriteStructuredClone(cmd)); return; } } LOGERROR("Invalid playerid in PostCommand!"); } static JS::Value ComputePath(ScriptInterface::CxPrivate* pCxPrivate, JS::HandleValue position, JS::HandleValue goal, pass_class_t passClass) { ENSURE(pCxPrivate->pCBData); CAIWorker* self = static_cast (pCxPrivate->pCBData); JSContext* cx(self->m_ScriptInterface->GetContext()); JSAutoRequest rq(cx); CFixedVector2D pos, goalPos; std::vector waypoints; JS::RootedValue retVal(cx); self->m_ScriptInterface->FromJSVal(cx, position, pos); self->m_ScriptInterface->FromJSVal(cx, goal, goalPos); self->ComputePath(pos, goalPos, passClass, waypoints); self->m_ScriptInterface->ToJSVal >(cx, &retVal, waypoints); return retVal; } void ComputePath(const CFixedVector2D& pos, const CFixedVector2D& goal, pass_class_t passClass, std::vector& waypoints) { WaypointPath ret; PathGoal pathGoal = { PathGoal::POINT, goal.X, goal.Y }; - m_LongPathfinder.ComputePath(pos.X, pos.Y, pathGoal, passClass, ret); + m_LongPathfinder.ComputePath(m_HierarchicalPathfinder, pos.X, pos.Y, pathGoal, passClass, ret); for (Waypoint& wp : ret.m_Waypoints) waypoints.emplace_back(wp.x, wp.z); } static CParamNode GetTemplate(ScriptInterface::CxPrivate* pCxPrivate, const std::string& name) { ENSURE(pCxPrivate->pCBData); CAIWorker* self = static_cast (pCxPrivate->pCBData); return self->GetTemplate(name); } CParamNode GetTemplate(const std::string& name) { if (!m_TemplateLoader.TemplateExists(name)) return CParamNode(false); return m_TemplateLoader.GetTemplateFileData(name).GetChild("Entity"); } static void ExitProgram(ScriptInterface::CxPrivate* UNUSED(pCxPrivate)) { QuitEngine(); } /** * Debug function for AI scripts to dump 2D array data (e.g. terrain tile weights). */ static void DumpImage(ScriptInterface::CxPrivate* UNUSED(pCxPrivate), const std::wstring& name, const std::vector& data, u32 w, u32 h, u32 max) { // TODO: this is totally not threadsafe. VfsPath filename = L"screenshots/aidump/" + name; if (data.size() != w*h) { debug_warn(L"DumpImage: data size doesn't match w*h"); return; } if (max == 0) { debug_warn(L"DumpImage: max must not be 0"); return; } const size_t bpp = 8; int flags = TEX_BOTTOM_UP|TEX_GREY; const size_t img_size = w * h * bpp/8; const size_t hdr_size = tex_hdr_size(filename); shared_ptr buf; AllocateAligned(buf, hdr_size+img_size, maxSectorSize); Tex t; if (t.wrap(w, h, bpp, flags, buf, hdr_size) < 0) return; u8* img = buf.get() + hdr_size; for (size_t i = 0; i < data.size(); ++i) img[i] = (u8)((data[i] * 255) / max); tex_write(&t, filename); } void SetRNGSeed(u32 seed) { m_RNG.seed(seed); } bool TryLoadSharedComponent() { JSContext* cx = m_ScriptInterface->GetContext(); JSAutoRequest rq(cx); // we don't need to load it. if (!m_HasSharedComponent) return false; // reset the value so it can be used to determine if we actually initialized it. m_HasSharedComponent = false; if (LoadScripts(L"common-api")) m_HasSharedComponent = true; else return false; // mainly here for the error messages OsPath path = L"simulation/ai/common-api/"; // Constructor name is SharedScript, it's in the module API3 // TODO: Hardcoding this is bad, we need a smarter way. JS::RootedValue AIModule(cx); JS::RootedValue global(cx, m_ScriptInterface->GetGlobalObject()); JS::RootedValue ctor(cx); if (!m_ScriptInterface->GetProperty(global, "API3", &AIModule) || AIModule.isUndefined()) { LOGERROR("Failed to create shared AI component: %s: can't find module '%s'", path.string8(), "API3"); return false; } if (!m_ScriptInterface->GetProperty(AIModule, "SharedScript", &ctor) || ctor.isUndefined()) { LOGERROR("Failed to create shared AI component: %s: can't find constructor '%s'", path.string8(), "SharedScript"); return false; } // Set up the data to pass as the constructor argument JS::RootedValue settings(cx); m_ScriptInterface->Eval(L"({})", &settings); JS::RootedValue playersID(cx); m_ScriptInterface->Eval(L"({})", &playersID); for (size_t i = 0; i < m_Players.size(); ++i) { JS::RootedValue val(cx); m_ScriptInterface->ToJSVal(cx, &val, m_Players[i]->m_Player); m_ScriptInterface->SetPropertyInt(playersID, i, val, true); } m_ScriptInterface->SetProperty(settings, "players", playersID); ENSURE(m_HasLoadedEntityTemplates); m_ScriptInterface->SetProperty(settings, "templates", m_EntityTemplates, false); JS::AutoValueVector argv(cx); argv.append(settings); m_ScriptInterface->CallConstructor(ctor, argv, &m_SharedAIObj); if (m_SharedAIObj.get().isNull()) { LOGERROR("Failed to create shared AI component: %s: error calling constructor '%s'", path.string8(), "SharedScript"); return false; } return true; } bool AddPlayer(const std::wstring& aiName, player_id_t player, u8 difficulty, const std::wstring& behavior) { shared_ptr ai(new CAIPlayer(*this, aiName, player, difficulty, behavior, m_ScriptInterface)); if (!ai->Initialise()) return false; // this will be set to true if we need to load the shared Component. if (!m_HasSharedComponent) m_HasSharedComponent = ai->m_UseSharedComponent; m_Players.push_back(ai); return true; } bool RunGamestateInit(const shared_ptr& gameState, const Grid& passabilityMap, const Grid& territoryMap, const std::map& nonPathfindingPassClassMasks, const std::map& pathfindingPassClassMasks) { // this will be run last by InitGame.js, passing the full game representation. // For now it will run for the shared Component. // This is NOT run during deserialization. JSContext* cx = m_ScriptInterface->GetContext(); JSAutoRequest rq(cx); JS::RootedValue state(cx); m_ScriptInterface->ReadStructuredClone(gameState, &state); ScriptInterface::ToJSVal(cx, &m_PassabilityMapVal, passabilityMap); ScriptInterface::ToJSVal(cx, &m_TerritoryMapVal, territoryMap); m_PassabilityMap = passabilityMap; m_NonPathfindingPassClasses = nonPathfindingPassClassMasks; m_PathfindingPassClasses = pathfindingPassClassMasks; - m_LongPathfinder.Reload(&m_PassabilityMap, nonPathfindingPassClassMasks, pathfindingPassClassMasks); + m_LongPathfinder.Reload(&m_PassabilityMap); + m_HierarchicalPathfinder.Recompute(&m_PassabilityMap, nonPathfindingPassClassMasks, pathfindingPassClassMasks); if (m_HasSharedComponent) { m_ScriptInterface->SetProperty(state, "passabilityMap", m_PassabilityMapVal, true); m_ScriptInterface->SetProperty(state, "territoryMap", m_TerritoryMapVal, true); m_ScriptInterface->CallFunctionVoid(m_SharedAIObj, "init", state); for (size_t i = 0; i < m_Players.size(); ++i) { if (m_HasSharedComponent && m_Players[i]->m_UseSharedComponent) m_Players[i]->InitAI(state, m_SharedAIObj); } } return true; } void UpdateGameState(const shared_ptr& gameState) { ENSURE(m_CommandsComputed); m_GameState = gameState; } void UpdatePathfinder(const Grid& passabilityMap, bool globallyDirty, const Grid& dirtinessGrid, bool justDeserialized, const std::map& nonPathfindingPassClassMasks, const std::map& pathfindingPassClassMasks) { ENSURE(m_CommandsComputed); bool dimensionChange = m_PassabilityMap.m_W != passabilityMap.m_W || m_PassabilityMap.m_H != passabilityMap.m_H; m_PassabilityMap = passabilityMap; if (globallyDirty) - m_LongPathfinder.Reload(&m_PassabilityMap, nonPathfindingPassClassMasks, pathfindingPassClassMasks); + { + m_LongPathfinder.Reload(&m_PassabilityMap); + m_HierarchicalPathfinder.Recompute(&m_PassabilityMap, nonPathfindingPassClassMasks, pathfindingPassClassMasks); + } else - m_LongPathfinder.Update(&m_PassabilityMap, dirtinessGrid); + { + m_LongPathfinder.Update(&m_PassabilityMap); + m_HierarchicalPathfinder.Update(&m_PassabilityMap, dirtinessGrid); + } JSContext* cx = m_ScriptInterface->GetContext(); if (dimensionChange || justDeserialized) ScriptInterface::ToJSVal(cx, &m_PassabilityMapVal, m_PassabilityMap); else { // Avoid a useless memory reallocation followed by a garbage collection. JSAutoRequest rq(cx); JS::RootedObject mapObj(cx, &m_PassabilityMapVal.toObject()); JS::RootedValue mapData(cx); ENSURE(JS_GetProperty(cx, mapObj, "data", &mapData)); JS::RootedObject dataObj(cx, &mapData.toObject()); u32 length = 0; ENSURE(JS_GetArrayLength(cx, dataObj, &length)); u32 nbytes = (u32)(length * sizeof(NavcellData)); JS::AutoCheckCannotGC nogc; memcpy((void*)JS_GetUint16ArrayData(dataObj, nogc), m_PassabilityMap.m_Data, nbytes); } } void UpdateTerritoryMap(const Grid& territoryMap) { ENSURE(m_CommandsComputed); bool dimensionChange = m_TerritoryMap.m_W != territoryMap.m_W || m_TerritoryMap.m_H != territoryMap.m_H; m_TerritoryMap = territoryMap; JSContext* cx = m_ScriptInterface->GetContext(); if (dimensionChange) ScriptInterface::ToJSVal(cx, &m_TerritoryMapVal, m_TerritoryMap); else { // Avoid a useless memory reallocation followed by a garbage collection. JSAutoRequest rq(cx); JS::RootedObject mapObj(cx, &m_TerritoryMapVal.toObject()); JS::RootedValue mapData(cx); ENSURE(JS_GetProperty(cx, mapObj, "data", &mapData)); JS::RootedObject dataObj(cx, &mapData.toObject()); u32 length = 0; ENSURE(JS_GetArrayLength(cx, dataObj, &length)); u32 nbytes = (u32)(length * sizeof(u8)); JS::AutoCheckCannotGC nogc; memcpy((void*)JS_GetUint8ArrayData(dataObj, nogc), m_TerritoryMap.m_Data, nbytes); } } void StartComputation() { m_CommandsComputed = false; } void WaitToFinishComputation() { if (!m_CommandsComputed) { PerformComputation(); m_CommandsComputed = true; } } void GetCommands(std::vector& commands) { WaitToFinishComputation(); commands.clear(); commands.resize(m_Players.size()); for (size_t i = 0; i < m_Players.size(); ++i) { commands[i].player = m_Players[i]->m_Player; commands[i].commands = m_Players[i]->m_Commands; } } void LoadEntityTemplates(const std::vector >& templates) { JSContext* cx = m_ScriptInterface->GetContext(); JSAutoRequest rq(cx); m_HasLoadedEntityTemplates = true; m_ScriptInterface->Eval("({})", &m_EntityTemplates); JS::RootedValue val(cx); for (size_t i = 0; i < templates.size(); ++i) { templates[i].second->ToJSVal(cx, false, &val); m_ScriptInterface->SetProperty(m_EntityTemplates, templates[i].first.c_str(), val, true); } } void Serialize(std::ostream& stream, bool isDebug) { WaitToFinishComputation(); if (isDebug) { CDebugSerializer serializer(*m_ScriptInterface, stream); serializer.Indent(4); SerializeState(serializer); } else { CStdSerializer serializer(*m_ScriptInterface, stream); // TODO: see comment in Deserialize() serializer.SetSerializablePrototypes(m_SerializablePrototypes); SerializeState(serializer); } } void SerializeState(ISerializer& serializer) { if (m_Players.empty()) return; JSContext* cx = m_ScriptInterface->GetContext(); JSAutoRequest rq(cx); std::stringstream rngStream; rngStream << m_RNG; serializer.StringASCII("rng", rngStream.str(), 0, 32); serializer.NumberU32_Unbounded("turn", m_TurnNum); serializer.Bool("useSharedScript", m_HasSharedComponent); if (m_HasSharedComponent) { JS::RootedValue sharedData(cx); if (!m_ScriptInterface->CallFunction(m_SharedAIObj, "Serialize", &sharedData)) LOGERROR("AI shared script Serialize call failed"); serializer.ScriptVal("sharedData", &sharedData); } for (size_t i = 0; i < m_Players.size(); ++i) { serializer.String("name", m_Players[i]->m_AIName, 1, 256); serializer.NumberI32_Unbounded("player", m_Players[i]->m_Player); serializer.NumberU8_Unbounded("difficulty", m_Players[i]->m_Difficulty); serializer.String("behavior", m_Players[i]->m_Behavior, 1, 256); serializer.NumberU32_Unbounded("num commands", (u32)m_Players[i]->m_Commands.size()); for (size_t j = 0; j < m_Players[i]->m_Commands.size(); ++j) { JS::RootedValue val(cx); m_ScriptInterface->ReadStructuredClone(m_Players[i]->m_Commands[j], &val); serializer.ScriptVal("command", &val); } bool hasCustomSerialize = m_ScriptInterface->HasProperty(m_Players[i]->m_Obj, "Serialize"); if (hasCustomSerialize) { JS::RootedValue scriptData(cx); if (!m_ScriptInterface->CallFunction(m_Players[i]->m_Obj, "Serialize", &scriptData)) LOGERROR("AI script Serialize call failed"); serializer.ScriptVal("data", &scriptData); } else { serializer.ScriptVal("data", &m_Players[i]->m_Obj); } } // AI pathfinder SerializeMap()(serializer, "non pathfinding pass classes", m_NonPathfindingPassClasses); SerializeMap()(serializer, "pathfinding pass classes", m_PathfindingPassClasses); serializer.NumberU16_Unbounded("pathfinder grid w", m_PassabilityMap.m_W); serializer.NumberU16_Unbounded("pathfinder grid h", m_PassabilityMap.m_H); serializer.RawBytes("pathfinder grid data", (const u8*)m_PassabilityMap.m_Data, m_PassabilityMap.m_W*m_PassabilityMap.m_H*sizeof(NavcellData)); } void Deserialize(std::istream& stream, u32 numAis) { m_PlayerMetadata.clear(); m_Players.clear(); if (numAis == 0) return; JSContext* cx = m_ScriptInterface->GetContext(); JSAutoRequest rq(cx); ENSURE(m_CommandsComputed); // deserializing while we're still actively computing would be bad CStdDeserializer deserializer(*m_ScriptInterface, stream); std::string rngString; std::stringstream rngStream; deserializer.StringASCII("rng", rngString, 0, 32); rngStream << rngString; rngStream >> m_RNG; deserializer.NumberU32_Unbounded("turn", m_TurnNum); deserializer.Bool("useSharedScript", m_HasSharedComponent); if (m_HasSharedComponent) { TryLoadSharedComponent(); JS::RootedValue sharedData(cx); deserializer.ScriptVal("sharedData", &sharedData); if (!m_ScriptInterface->CallFunctionVoid(m_SharedAIObj, "Deserialize", sharedData)) LOGERROR("AI shared script Deserialize call failed"); } for (size_t i = 0; i < numAis; ++i) { std::wstring name; player_id_t player; u8 difficulty; std::wstring behavior; deserializer.String("name", name, 1, 256); deserializer.NumberI32_Unbounded("player", player); deserializer.NumberU8_Unbounded("difficulty",difficulty); deserializer.String("behavior", behavior, 1, 256); if (!AddPlayer(name, player, difficulty, behavior)) throw PSERROR_Deserialize_ScriptError(); u32 numCommands; deserializer.NumberU32_Unbounded("num commands", numCommands); m_Players.back()->m_Commands.reserve(numCommands); for (size_t j = 0; j < numCommands; ++j) { JS::RootedValue val(cx); deserializer.ScriptVal("command", &val); m_Players.back()->m_Commands.push_back(m_ScriptInterface->WriteStructuredClone(val)); } // TODO: this is yucky but necessary while the AIs are sharing data between contexts; // ideally a new (de)serializer instance would be created for each player // so they would have a single, consistent script context to use and serializable // prototypes could be stored in their ScriptInterface deserializer.SetSerializablePrototypes(m_DeserializablePrototypes); bool hasCustomDeserialize = m_ScriptInterface->HasProperty(m_Players.back()->m_Obj, "Deserialize"); if (hasCustomDeserialize) { JS::RootedValue scriptData(cx); deserializer.ScriptVal("data", &scriptData); if (m_Players[i]->m_UseSharedComponent) { if (!m_ScriptInterface->CallFunctionVoid(m_Players.back()->m_Obj, "Deserialize", scriptData, m_SharedAIObj)) LOGERROR("AI script Deserialize call failed"); } else if (!m_ScriptInterface->CallFunctionVoid(m_Players.back()->m_Obj, "Deserialize", scriptData)) { LOGERROR("AI script deserialize() call failed"); } } else { deserializer.ScriptVal("data", &m_Players.back()->m_Obj); } } // AI pathfinder SerializeMap()(deserializer, "non pathfinding pass classes", m_NonPathfindingPassClasses); SerializeMap()(deserializer, "pathfinding pass classes", m_PathfindingPassClasses); u16 mapW, mapH; deserializer.NumberU16_Unbounded("pathfinder grid w", mapW); deserializer.NumberU16_Unbounded("pathfinder grid h", mapH); m_PassabilityMap = Grid(mapW, mapH); deserializer.RawBytes("pathfinder grid data", (u8*)m_PassabilityMap.m_Data, mapW*mapH*sizeof(NavcellData)); - m_LongPathfinder.Reload(&m_PassabilityMap, m_NonPathfindingPassClasses, m_PathfindingPassClasses); + m_LongPathfinder.Reload(&m_PassabilityMap); + m_HierarchicalPathfinder.Recompute(&m_PassabilityMap, m_NonPathfindingPassClasses, m_PathfindingPassClasses); } int getPlayerSize() { return m_Players.size(); } void RegisterSerializablePrototype(std::wstring name, JS::HandleValue proto) { // Require unique prototype and name (for reverse lookup) // TODO: this is yucky - see comment in Deserialize() ENSURE(proto.isObject() && "A serializable prototype has to be an object!"); JSContext* cx = m_ScriptInterface->GetContext(); JSAutoRequest rq(cx); JS::RootedObject obj(cx, &proto.toObject()); if (m_SerializablePrototypes->has(obj) || m_DeserializablePrototypes.find(name) != m_DeserializablePrototypes.end()) { LOGERROR("RegisterSerializablePrototype called with same prototype multiple times: p=%p n='%s'", (void *)obj.get(), utf8_from_wstring(name)); return; } m_SerializablePrototypes->add(cx, obj, name); m_DeserializablePrototypes[name] = JS::Heap(obj); } private: static void Trace(JSTracer *trc, void *data) { reinterpret_cast(data)->TraceMember(trc); } void TraceMember(JSTracer *trc) { for (std::pair>& prototype : m_DeserializablePrototypes) JS_CallObjectTracer(trc, &prototype.second, "CAIWorker::m_DeserializablePrototypes"); for (std::pair>& metadata : m_PlayerMetadata) JS_CallValueTracer(trc, &metadata.second, "CAIWorker::m_PlayerMetadata"); } void LoadMetadata(const VfsPath& path, JS::MutableHandleValue out) { if (m_PlayerMetadata.find(path) == m_PlayerMetadata.end()) { // Load and cache the AI player metadata m_ScriptInterface->ReadJSONFile(path, out); m_PlayerMetadata[path] = JS::Heap(out); return; } out.set(m_PlayerMetadata[path].get()); } void PerformComputation() { // Deserialize the game state, to pass to the AI's HandleMessage JSContext* cx = m_ScriptInterface->GetContext(); JSAutoRequest rq(cx); JS::RootedValue state(cx); { PROFILE3("AI compute read state"); m_ScriptInterface->ReadStructuredClone(m_GameState, &state); m_ScriptInterface->SetProperty(state, "passabilityMap", m_PassabilityMapVal, true); m_ScriptInterface->SetProperty(state, "territoryMap", m_TerritoryMapVal, true); } // It would be nice to do // m_ScriptInterface->FreezeObject(state.get(), true); // to prevent AI scripts accidentally modifying the state and // affecting other AI scripts they share it with. But the performance // cost is far too high, so we won't do that. // If there is a shared component, run it if (m_HasSharedComponent) { PROFILE3("AI run shared component"); m_ScriptInterface->CallFunctionVoid(m_SharedAIObj, "onUpdate", state); } for (size_t i = 0; i < m_Players.size(); ++i) { PROFILE3("AI script"); PROFILE2_ATTR("player: %d", m_Players[i]->m_Player); PROFILE2_ATTR("script: %ls", m_Players[i]->m_AIName.c_str()); if (m_HasSharedComponent && m_Players[i]->m_UseSharedComponent) m_Players[i]->Run(state, m_Players[i]->m_Player, m_SharedAIObj); else m_Players[i]->Run(state, m_Players[i]->m_Player); } } // Take care to keep this declaration before heap rooted members. Destructors of heap rooted // members have to be called before the runtime destructor. shared_ptr m_ScriptRuntime; shared_ptr m_ScriptInterface; boost::rand48 m_RNG; u32 m_TurnNum; JS::PersistentRootedValue m_EntityTemplates; bool m_HasLoadedEntityTemplates; std::map > m_PlayerMetadata; std::vector > m_Players; // use shared_ptr just to avoid copying bool m_HasSharedComponent; JS::PersistentRootedValue m_SharedAIObj; std::vector m_Commands; std::set m_LoadedModules; shared_ptr m_GameState; Grid m_PassabilityMap; JS::PersistentRootedValue m_PassabilityMapVal; Grid m_TerritoryMap; JS::PersistentRootedValue m_TerritoryMapVal; std::map m_NonPathfindingPassClasses; std::map m_PathfindingPassClasses; + HierarchicalPathfinder m_HierarchicalPathfinder; LongPathfinder m_LongPathfinder; bool m_CommandsComputed; shared_ptr > m_SerializablePrototypes; std::map > m_DeserializablePrototypes; CTemplateLoader m_TemplateLoader; }; /** * Implementation of ICmpAIManager. */ class CCmpAIManager : public ICmpAIManager { public: static void ClassInit(CComponentManager& UNUSED(componentManager)) { } DEFAULT_COMPONENT_ALLOCATOR(AIManager) static std::string GetSchema() { return ""; } virtual void Init(const CParamNode& UNUSED(paramNode)) { m_TerritoriesDirtyID = 0; m_TerritoriesDirtyBlinkingID = 0; m_JustDeserialized = false; } virtual void Deinit() { } virtual void Serialize(ISerializer& serialize) { serialize.NumberU32_Unbounded("num ais", m_Worker.getPlayerSize()); // Because the AI worker uses its own ScriptInterface, we can't use the // ISerializer (which was initialised with the simulation ScriptInterface) // directly. So we'll just grab the ISerializer's stream and write to it // with an independent serializer. m_Worker.Serialize(serialize.GetStream(), serialize.IsDebug()); } virtual void Deserialize(const CParamNode& paramNode, IDeserializer& deserialize) { Init(paramNode); u32 numAis; deserialize.NumberU32_Unbounded("num ais", numAis); if (numAis > 0) LoadUsedEntityTemplates(); m_Worker.Deserialize(deserialize.GetStream(), numAis); m_JustDeserialized = true; } virtual void AddPlayer(const std::wstring& id, player_id_t player, u8 difficulty, const std::wstring& behavior) { LoadUsedEntityTemplates(); m_Worker.AddPlayer(id, player, difficulty, behavior); // AI players can cheat and see through FoW/SoD, since that greatly simplifies // their implementation. // (TODO: maybe cleverer AIs should be able to optionally retain FoW/SoD) CmpPtr cmpRangeManager(GetSystemEntity()); if (cmpRangeManager) cmpRangeManager->SetLosRevealAll(player, true); } virtual void SetRNGSeed(u32 seed) { m_Worker.SetRNGSeed(seed); } virtual void TryLoadSharedComponent() { m_Worker.TryLoadSharedComponent(); } virtual void RunGamestateInit() { const ScriptInterface& scriptInterface = GetSimContext().GetScriptInterface(); JSContext* cx = scriptInterface.GetContext(); JSAutoRequest rq(cx); CmpPtr cmpAIInterface(GetSystemEntity()); ENSURE(cmpAIInterface); // Get the game state from AIInterface // We flush events from the initialization so we get a clean state now. JS::RootedValue state(cx); cmpAIInterface->GetFullRepresentation(&state, true); // Get the passability data Grid dummyGrid; const Grid* passabilityMap = &dummyGrid; CmpPtr cmpPathfinder(GetSystemEntity()); if (cmpPathfinder) passabilityMap = &cmpPathfinder->GetPassabilityGrid(); // Get the territory data // Since getting the territory grid can trigger a recalculation, we check NeedUpdateAI first Grid dummyGrid2; const Grid* territoryMap = &dummyGrid2; CmpPtr cmpTerritoryManager(GetSystemEntity()); if (cmpTerritoryManager && cmpTerritoryManager->NeedUpdateAI(&m_TerritoriesDirtyID, &m_TerritoriesDirtyBlinkingID)) territoryMap = &cmpTerritoryManager->GetTerritoryGrid(); LoadPathfinderClasses(state); std::map nonPathfindingPassClassMasks, pathfindingPassClassMasks; if (cmpPathfinder) cmpPathfinder->GetPassabilityClasses(nonPathfindingPassClassMasks, pathfindingPassClassMasks); m_Worker.RunGamestateInit(scriptInterface.WriteStructuredClone(state), *passabilityMap, *territoryMap, nonPathfindingPassClassMasks, pathfindingPassClassMasks); } virtual void StartComputation() { PROFILE("AI setup"); const ScriptInterface& scriptInterface = GetSimContext().GetScriptInterface(); JSContext* cx = scriptInterface.GetContext(); JSAutoRequest rq(cx); if (m_Worker.getPlayerSize() == 0) return; CmpPtr cmpAIInterface(GetSystemEntity()); ENSURE(cmpAIInterface); // Get the game state from AIInterface JS::RootedValue state(cx); if (m_JustDeserialized) cmpAIInterface->GetFullRepresentation(&state, false); else cmpAIInterface->GetRepresentation(&state); LoadPathfinderClasses(state); // add the pathfinding classes to it // Update the game state m_Worker.UpdateGameState(scriptInterface.WriteStructuredClone(state)); // Update the pathfinding data CmpPtr cmpPathfinder(GetSystemEntity()); if (cmpPathfinder) { const GridUpdateInformation& dirtinessInformations = cmpPathfinder->GetAIPathfinderDirtinessInformation(); if (dirtinessInformations.dirty || m_JustDeserialized) { const Grid& passabilityMap = cmpPathfinder->GetPassabilityGrid(); std::map nonPathfindingPassClassMasks, pathfindingPassClassMasks; cmpPathfinder->GetPassabilityClasses(nonPathfindingPassClassMasks, pathfindingPassClassMasks); m_Worker.UpdatePathfinder(passabilityMap, dirtinessInformations.globallyDirty, dirtinessInformations.dirtinessGrid, m_JustDeserialized, nonPathfindingPassClassMasks, pathfindingPassClassMasks); } cmpPathfinder->FlushAIPathfinderDirtinessInformation(); } // Update the territory data // Since getting the territory grid can trigger a recalculation, we check NeedUpdateAI first CmpPtr cmpTerritoryManager(GetSystemEntity()); if (cmpTerritoryManager && (cmpTerritoryManager->NeedUpdateAI(&m_TerritoriesDirtyID, &m_TerritoriesDirtyBlinkingID) || m_JustDeserialized)) { const Grid& territoryMap = cmpTerritoryManager->GetTerritoryGrid(); m_Worker.UpdateTerritoryMap(territoryMap); } m_Worker.StartComputation(); m_JustDeserialized = false; } virtual void PushCommands() { std::vector commands; m_Worker.GetCommands(commands); CmpPtr cmpCommandQueue(GetSystemEntity()); if (!cmpCommandQueue) return; const ScriptInterface& scriptInterface = GetSimContext().GetScriptInterface(); JSContext* cx = scriptInterface.GetContext(); JSAutoRequest rq(cx); JS::RootedValue clonedCommandVal(cx); for (size_t i = 0; i < commands.size(); ++i) { for (size_t j = 0; j < commands[i].commands.size(); ++j) { scriptInterface.ReadStructuredClone(commands[i].commands[j], &clonedCommandVal); cmpCommandQueue->PushLocalCommand(commands[i].player, clonedCommandVal); } } } private: size_t m_TerritoriesDirtyID; size_t m_TerritoriesDirtyBlinkingID; bool m_JustDeserialized; /** * Load the templates of all entities on the map (called when adding a new AI player for a new game * or when deserializing) */ void LoadUsedEntityTemplates() { if (m_Worker.HasLoadedEntityTemplates()) return; CmpPtr cmpTemplateManager(GetSystemEntity()); ENSURE(cmpTemplateManager); std::vector templateNames = cmpTemplateManager->FindUsedTemplates(); std::vector > usedTemplates; usedTemplates.reserve(templateNames.size()); for (const std::string& name : templateNames) { const CParamNode* node = cmpTemplateManager->GetTemplateWithoutValidation(name); if (node) usedTemplates.emplace_back(name, node); } // Send the data to the worker m_Worker.LoadEntityTemplates(usedTemplates); } void LoadPathfinderClasses(JS::HandleValue state) { CmpPtr cmpPathfinder(GetSystemEntity()); if (!cmpPathfinder) return; const ScriptInterface& scriptInterface = GetSimContext().GetScriptInterface(); JSContext* cx = scriptInterface.GetContext(); JSAutoRequest rq(cx); JS::RootedValue classesVal(cx); scriptInterface.Eval("({})", &classesVal); std::map classes; cmpPathfinder->GetPassabilityClasses(classes); for (std::map::iterator it = classes.begin(); it != classes.end(); ++it) scriptInterface.SetProperty(classesVal, it->first.c_str(), it->second, true); scriptInterface.SetProperty(state, "passabilityClasses", classesVal, true); } CAIWorker m_Worker; }; REGISTER_COMPONENT_TYPE(AIManager) Index: ps/trunk/source/simulation2/components/CCmpPathfinder.cpp =================================================================== --- ps/trunk/source/simulation2/components/CCmpPathfinder.cpp (revision 22277) +++ ps/trunk/source/simulation2/components/CCmpPathfinder.cpp (revision 22278) @@ -1,916 +1,930 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ /** * @file * Common code and setup code for CCmpPathfinder. */ #include "precompiled.h" #include "CCmpPathfinder_Common.h" #include "ps/CLogger.h" #include "ps/CStr.h" #include "ps/Profile.h" #include "ps/XML/Xeromyces.h" #include "renderer/Scene.h" #include "simulation2/MessageTypes.h" #include "simulation2/components/ICmpObstruction.h" #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/components/ICmpTerrain.h" #include "simulation2/components/ICmpWaterManager.h" +#include "simulation2/helpers/HierarchicalPathfinder.h" +#include "simulation2/helpers/LongPathfinder.h" #include "simulation2/helpers/Rasterize.h" #include "simulation2/helpers/VertexPathfinder.h" #include "simulation2/serialization/SerializeTemplates.h" REGISTER_COMPONENT_TYPE(Pathfinder) void CCmpPathfinder::Init(const CParamNode& UNUSED(paramNode)) { m_MapSize = 0; m_Grid = NULL; m_TerrainOnlyGrid = NULL; FlushAIPathfinderDirtinessInformation(); m_NextAsyncTicket = 1; m_AtlasOverlay = NULL; m_SameTurnMovesCount = 0; m_VertexPathfinder = std::unique_ptr(new VertexPathfinder(m_MapSize, m_TerrainOnlyGrid)); + m_LongPathfinder = std::unique_ptr(new LongPathfinder()); + m_PathfinderHier = std::unique_ptr(new HierarchicalPathfinder()); // Register Relax NG validator CXeromyces::AddValidator(g_VFS, "pathfinder", "simulation/data/pathfinder.rng"); // Since this is used as a system component (not loaded from an entity template), // we can't use the real paramNode (it won't get handled properly when deserializing), // so load the data from a special XML file. CParamNode externalParamNode; CParamNode::LoadXML(externalParamNode, L"simulation/data/pathfinder.xml", "pathfinder"); // Previously all move commands during a turn were // queued up and processed asynchronously at the start // of the next turn. Now we are processing queued up // events several times duing the turn. This improves // responsiveness and units move more smoothly especially. // when in formation. There is still a call at the // beginning of a turn to process all outstanding moves - // this will handle any moves above the MaxSameTurnMoves // threshold. // // TODO - The moves processed at the beginning of the // turn do not count against the maximum moves per turn // currently. The thinking is that this will eventually // happen in another thread. Either way this probably // will require some adjustment and rethinking. const CParamNode pathingSettings = externalParamNode.GetChild("Pathfinder"); m_MaxSameTurnMoves = (u16)pathingSettings.GetChild("MaxSameTurnMoves").ToInt(); const CParamNode::ChildrenMap& passClasses = externalParamNode.GetChild("Pathfinder").GetChild("PassabilityClasses").GetChildren(); for (CParamNode::ChildrenMap::const_iterator it = passClasses.begin(); it != passClasses.end(); ++it) { std::string name = it->first; ENSURE((int)m_PassClasses.size() <= PASS_CLASS_BITS); pass_class_t mask = PASS_CLASS_MASK_FROM_INDEX(m_PassClasses.size()); m_PassClasses.push_back(PathfinderPassability(mask, it->second)); m_PassClassMasks[name] = mask; } } CCmpPathfinder::~CCmpPathfinder() {}; void CCmpPathfinder::Deinit() { SetDebugOverlay(false); // cleans up memory SAFE_DELETE(m_AtlasOverlay); SAFE_DELETE(m_Grid); SAFE_DELETE(m_TerrainOnlyGrid); } struct SerializeLongRequest { template void operator()(S& serialize, const char* UNUSED(name), AsyncLongPathRequest& value) { serialize.NumberU32_Unbounded("ticket", value.ticket); serialize.NumberFixed_Unbounded("x0", value.x0); serialize.NumberFixed_Unbounded("z0", value.z0); SerializeGoal()(serialize, "goal", value.goal); serialize.NumberU16_Unbounded("pass class", value.passClass); serialize.NumberU32_Unbounded("notify", value.notify); } }; struct SerializeShortRequest { template void operator()(S& serialize, const char* UNUSED(name), AsyncShortPathRequest& value) { serialize.NumberU32_Unbounded("ticket", value.ticket); serialize.NumberFixed_Unbounded("x0", value.x0); serialize.NumberFixed_Unbounded("z0", value.z0); serialize.NumberFixed_Unbounded("clearance", value.clearance); serialize.NumberFixed_Unbounded("range", value.range); SerializeGoal()(serialize, "goal", value.goal); serialize.NumberU16_Unbounded("pass class", value.passClass); serialize.Bool("avoid moving units", value.avoidMovingUnits); serialize.NumberU32_Unbounded("group", value.group); serialize.NumberU32_Unbounded("notify", value.notify); } }; template void CCmpPathfinder::SerializeCommon(S& serialize) { SerializeVector()(serialize, "long requests", m_AsyncLongPathRequests); SerializeVector()(serialize, "short requests", m_AsyncShortPathRequests); serialize.NumberU32_Unbounded("next ticket", m_NextAsyncTicket); serialize.NumberU16_Unbounded("same turn moves count", m_SameTurnMovesCount); serialize.NumberU16_Unbounded("map size", m_MapSize); } void CCmpPathfinder::Serialize(ISerializer& serialize) { SerializeCommon(serialize); } void CCmpPathfinder::Deserialize(const CParamNode& paramNode, IDeserializer& deserialize) { Init(paramNode); SerializeCommon(deserialize); } void CCmpPathfinder::HandleMessage(const CMessage& msg, bool UNUSED(global)) { switch (msg.GetType()) { case MT_RenderSubmit: { const CMessageRenderSubmit& msgData = static_cast (msg); RenderSubmit(msgData.collector); break; } case MT_TerrainChanged: m_TerrainDirty = true; MinimalTerrainUpdate(); break; case MT_WaterChanged: case MT_ObstructionMapShapeChanged: m_TerrainDirty = true; UpdateGrid(); break; case MT_TurnStart: m_SameTurnMovesCount = 0; break; } } void CCmpPathfinder::RenderSubmit(SceneCollector& collector) { m_VertexPathfinder->RenderSubmit(collector); - m_LongPathfinder.HierarchicalRenderSubmit(collector); + m_PathfinderHier->RenderSubmit(collector); } void CCmpPathfinder::SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) { - m_LongPathfinder.SetDebugPath(x0, z0, goal, passClass); + m_LongPathfinder->SetDebugPath(*m_PathfinderHier, x0, z0, goal, passClass); } void CCmpPathfinder::SetDebugOverlay(bool enabled) { m_VertexPathfinder->SetDebugOverlay(enabled); - m_LongPathfinder.SetDebugOverlay(enabled); + m_LongPathfinder->SetDebugOverlay(enabled); } void CCmpPathfinder::SetHierDebugOverlay(bool enabled) { - m_LongPathfinder.SetHierDebugOverlay(enabled, &GetSimContext()); + m_PathfinderHier->SetDebugOverlay(enabled, &GetSimContext()); } void CCmpPathfinder::GetDebugData(u32& steps, double& time, Grid& grid) const { - m_LongPathfinder.GetDebugData(steps, time, grid); + m_LongPathfinder->GetDebugData(steps, time, grid); } void CCmpPathfinder::SetAtlasOverlay(bool enable, pass_class_t passClass) { if (enable) { if (!m_AtlasOverlay) m_AtlasOverlay = new AtlasOverlay(this, passClass); m_AtlasOverlay->m_PassClass = passClass; } else SAFE_DELETE(m_AtlasOverlay); } pass_class_t CCmpPathfinder::GetPassabilityClass(const std::string& name) const { std::map::const_iterator it = m_PassClassMasks.find(name); if (it == m_PassClassMasks.end()) { LOGERROR("Invalid passability class name '%s'", name.c_str()); return 0; } return it->second; } void CCmpPathfinder::GetPassabilityClasses(std::map& passClasses) const { passClasses = m_PassClassMasks; } void CCmpPathfinder::GetPassabilityClasses(std::map& nonPathfindingPassClasses, std::map& pathfindingPassClasses) const { for (const std::pair& pair : m_PassClassMasks) { if ((GetPassabilityFromMask(pair.second)->m_Obstructions == PathfinderPassability::PATHFINDING)) pathfindingPassClasses[pair.first] = pair.second; else nonPathfindingPassClasses[pair.first] = pair.second; } } const PathfinderPassability* CCmpPathfinder::GetPassabilityFromMask(pass_class_t passClass) const { for (const PathfinderPassability& passability : m_PassClasses) { if (passability.m_Mask == passClass) return &passability; } return NULL; } const Grid& CCmpPathfinder::GetPassabilityGrid() { if (!m_Grid) UpdateGrid(); return *m_Grid; } /** * Given a grid of passable/impassable navcells (based on some passability mask), * computes a new grid where a navcell is impassable (per that mask) if * it is <=clearance navcells away from an impassable navcell in the original grid. * The results are ORed onto the original grid. * * This is used for adding clearance onto terrain-based navcell passability. * * TODO PATHFINDER: might be nicer to get rounded corners by measuring clearances as * Euclidean distances; currently it effectively does dist=max(dx,dy) instead. * This would only really be a problem for big clearances. */ static void ExpandImpassableCells(Grid& grid, u16 clearance, pass_class_t mask) { PROFILE3("ExpandImpassableCells"); u16 w = grid.m_W; u16 h = grid.m_H; // First expand impassable cells horizontally into a temporary 1-bit grid Grid tempGrid(w, h); for (u16 j = 0; j < h; ++j) { // New cell (i,j) is blocked if (i',j) blocked for any i-clearance <= i' <= i+clearance // Count the number of blocked cells around i=0 u16 numBlocked = 0; for (u16 i = 0; i <= clearance && i < w; ++i) if (!IS_PASSABLE(grid.get(i, j), mask)) ++numBlocked; for (u16 i = 0; i < w; ++i) { // Store a flag if blocked by at least one nearby cell if (numBlocked) tempGrid.set(i, j, 1); // Slide the numBlocked window along: // remove the old i-clearance value, add the new (i+1)+clearance // (avoiding overflowing the grid) if (i >= clearance && !IS_PASSABLE(grid.get(i-clearance, j), mask)) --numBlocked; if (i+1+clearance < w && !IS_PASSABLE(grid.get(i+1+clearance, j), mask)) ++numBlocked; } } for (u16 i = 0; i < w; ++i) { // New cell (i,j) is blocked if (i,j') blocked for any j-clearance <= j' <= j+clearance // Count the number of blocked cells around j=0 u16 numBlocked = 0; for (u16 j = 0; j <= clearance && j < h; ++j) if (tempGrid.get(i, j)) ++numBlocked; for (u16 j = 0; j < h; ++j) { // Add the mask if blocked by at least one nearby cell if (numBlocked) grid.set(i, j, grid.get(i, j) | mask); // Slide the numBlocked window along: // remove the old j-clearance value, add the new (j+1)+clearance // (avoiding overflowing the grid) if (j >= clearance && tempGrid.get(i, j-clearance)) --numBlocked; if (j+1+clearance < h && tempGrid.get(i, j+1+clearance)) ++numBlocked; } } } Grid CCmpPathfinder::ComputeShoreGrid(bool expandOnWater) { PROFILE3("ComputeShoreGrid"); CmpPtr cmpWaterManager(GetSystemEntity()); // TODO: these bits should come from ICmpTerrain CTerrain& terrain = GetSimContext().GetTerrain(); // avoid integer overflow in intermediate calculation const u16 shoreMax = 32767; // First pass - find underwater tiles Grid waterGrid(m_MapSize, m_MapSize); for (u16 j = 0; j < m_MapSize; ++j) { for (u16 i = 0; i < m_MapSize; ++i) { fixed x, z; Pathfinding::TileCenter(i, j, x, z); bool underWater = cmpWaterManager && (cmpWaterManager->GetWaterLevel(x, z) > terrain.GetExactGroundLevelFixed(x, z)); waterGrid.set(i, j, underWater ? 1 : 0); } } // Second pass - find shore tiles Grid shoreGrid(m_MapSize, m_MapSize); for (u16 j = 0; j < m_MapSize; ++j) { for (u16 i = 0; i < m_MapSize; ++i) { // Find a land tile if (!waterGrid.get(i, j)) { // If it's bordered by water, it's a shore tile if ((i > 0 && waterGrid.get(i-1, j)) || (i > 0 && j < m_MapSize-1 && waterGrid.get(i-1, j+1)) || (i > 0 && j > 0 && waterGrid.get(i-1, j-1)) || (i < m_MapSize-1 && waterGrid.get(i+1, j)) || (i < m_MapSize-1 && j < m_MapSize-1 && waterGrid.get(i+1, j+1)) || (i < m_MapSize-1 && j > 0 && waterGrid.get(i+1, j-1)) || (j > 0 && waterGrid.get(i, j-1)) || (j < m_MapSize-1 && waterGrid.get(i, j+1)) ) shoreGrid.set(i, j, 0); else shoreGrid.set(i, j, shoreMax); } // If we want to expand on water, we want water tiles not to be shore tiles else if (expandOnWater) shoreGrid.set(i, j, shoreMax); } } // Expand influences on land to find shore distance for (u16 y = 0; y < m_MapSize; ++y) { u16 min = shoreMax; for (u16 x = 0; x < m_MapSize; ++x) { if (!waterGrid.get(x, y) || expandOnWater) { u16 g = shoreGrid.get(x, y); if (g > min) shoreGrid.set(x, y, min); else if (g < min) min = g; ++min; } } for (u16 x = m_MapSize; x > 0; --x) { if (!waterGrid.get(x-1, y) || expandOnWater) { u16 g = shoreGrid.get(x-1, y); if (g > min) shoreGrid.set(x-1, y, min); else if (g < min) min = g; ++min; } } } for (u16 x = 0; x < m_MapSize; ++x) { u16 min = shoreMax; for (u16 y = 0; y < m_MapSize; ++y) { if (!waterGrid.get(x, y) || expandOnWater) { u16 g = shoreGrid.get(x, y); if (g > min) shoreGrid.set(x, y, min); else if (g < min) min = g; ++min; } } for (u16 y = m_MapSize; y > 0; --y) { if (!waterGrid.get(x, y-1) || expandOnWater) { u16 g = shoreGrid.get(x, y-1); if (g > min) shoreGrid.set(x, y-1, min); else if (g < min) min = g; ++min; } } } return shoreGrid; } void CCmpPathfinder::UpdateGrid() { PROFILE3("UpdateGrid"); CmpPtr cmpTerrain(GetSimContext(), SYSTEM_ENTITY); if (!cmpTerrain) return; // error u16 terrainSize = cmpTerrain->GetTilesPerSide(); if (terrainSize == 0) return; // If the terrain was resized then delete the old grid data if (m_Grid && m_MapSize != terrainSize) { SAFE_DELETE(m_Grid); SAFE_DELETE(m_TerrainOnlyGrid); } // Initialise the terrain data when first needed if (!m_Grid) { m_MapSize = terrainSize; m_Grid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); SAFE_DELETE(m_TerrainOnlyGrid); m_TerrainOnlyGrid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); m_DirtinessInformation = { true, true, Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE) }; m_AIPathfinderDirtinessInformation = m_DirtinessInformation; m_TerrainDirty = true; } // The grid should be properly initialized and clean. Checking the latter is expensive so do it only for debugging. #ifdef NDEBUG ENSURE(m_DirtinessInformation.dirtinessGrid.compare_sizes(m_Grid)); #else ENSURE(m_DirtinessInformation.dirtinessGrid == Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE)); #endif CmpPtr cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY); cmpObstructionManager->UpdateInformations(m_DirtinessInformation); if (!m_DirtinessInformation.dirty && !m_TerrainDirty) return; // If the terrain has changed, recompute m_Grid // Else, use data from m_TerrainOnlyGrid and add obstructions if (m_TerrainDirty) { TerrainUpdateHelper(); *m_Grid = *m_TerrainOnlyGrid; m_TerrainDirty = false; m_DirtinessInformation.globallyDirty = true; } else if (m_DirtinessInformation.globallyDirty) { ENSURE(m_Grid->compare_sizes(m_TerrainOnlyGrid)); memcpy(m_Grid->m_Data, m_TerrainOnlyGrid->m_Data, (m_Grid->m_W)*(m_Grid->m_H)*sizeof(NavcellData)); } else { ENSURE(m_Grid->compare_sizes(m_TerrainOnlyGrid)); for (u16 j = 0; j < m_DirtinessInformation.dirtinessGrid.m_H; ++j) for (u16 i = 0; i < m_DirtinessInformation.dirtinessGrid.m_W; ++i) if (m_DirtinessInformation.dirtinessGrid.get(i, j) == 1) m_Grid->set(i, j, m_TerrainOnlyGrid->get(i, j)); } // Add obstructions onto the grid cmpObstructionManager->Rasterize(*m_Grid, m_PassClasses, m_DirtinessInformation.globallyDirty); - // Update the long-range pathfinder + // Update the long-range and hierarchical pathfinders. if (m_DirtinessInformation.globallyDirty) { std::map nonPathfindingPassClasses, pathfindingPassClasses; GetPassabilityClasses(nonPathfindingPassClasses, pathfindingPassClasses); - m_LongPathfinder.Reload(m_Grid, nonPathfindingPassClasses, pathfindingPassClasses); + m_LongPathfinder->Reload(m_Grid); + m_PathfinderHier->Recompute(m_Grid, nonPathfindingPassClasses, pathfindingPassClasses); } else - m_LongPathfinder.Update(m_Grid, m_DirtinessInformation.dirtinessGrid); + { + m_LongPathfinder->Update(m_Grid); + m_PathfinderHier->Update(m_Grid, m_DirtinessInformation.dirtinessGrid); + } // Remember the necessary updates that the AI pathfinder will have to perform as well m_AIPathfinderDirtinessInformation.MergeAndClear(m_DirtinessInformation); } void CCmpPathfinder::MinimalTerrainUpdate() { TerrainUpdateHelper(false); } void CCmpPathfinder::TerrainUpdateHelper(bool expandPassability/* = true */) { PROFILE3("TerrainUpdateHelper"); CmpPtr cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY); CmpPtr cmpWaterManager(GetSimContext(), SYSTEM_ENTITY); CmpPtr cmpTerrain(GetSimContext(), SYSTEM_ENTITY); CTerrain& terrain = GetSimContext().GetTerrain(); if (!cmpTerrain || !cmpObstructionManager) return; u16 terrainSize = cmpTerrain->GetTilesPerSide(); if (terrainSize == 0) return; if (!m_TerrainOnlyGrid || m_MapSize != terrainSize) { m_MapSize = terrainSize; SAFE_DELETE(m_TerrainOnlyGrid); m_TerrainOnlyGrid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); // If this update comes from a map resizing, we must reinitialize the other grids as well if (!m_TerrainOnlyGrid->compare_sizes(m_Grid)) { SAFE_DELETE(m_Grid); m_Grid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); m_DirtinessInformation = { true, true, Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE) }; m_AIPathfinderDirtinessInformation = m_DirtinessInformation; } } Grid shoreGrid = ComputeShoreGrid(); // Compute initial terrain-dependent passability for (int j = 0; j < m_MapSize * Pathfinding::NAVCELLS_PER_TILE; ++j) { for (int i = 0; i < m_MapSize * Pathfinding::NAVCELLS_PER_TILE; ++i) { // World-space coordinates for this navcell fixed x, z; Pathfinding::NavcellCenter(i, j, x, z); // Terrain-tile coordinates for this navcell int itile = i / Pathfinding::NAVCELLS_PER_TILE; int jtile = j / Pathfinding::NAVCELLS_PER_TILE; // Gather all the data potentially needed to determine passability: fixed height = terrain.GetExactGroundLevelFixed(x, z); fixed water; if (cmpWaterManager) water = cmpWaterManager->GetWaterLevel(x, z); fixed depth = water - height; // Exact slopes give kind of weird output, so just use rough tile-based slopes fixed slope = terrain.GetSlopeFixed(itile, jtile); // Get world-space coordinates from shoreGrid (which uses terrain tiles) fixed shoredist = fixed::FromInt(shoreGrid.get(itile, jtile)).MultiplyClamp(TERRAIN_TILE_SIZE); // Compute the passability for every class for this cell NavcellData t = 0; for (PathfinderPassability& passability : m_PassClasses) if (!passability.IsPassable(depth, slope, shoredist)) t |= passability.m_Mask; m_TerrainOnlyGrid->set(i, j, t); } } // Compute off-world passability // WARNING: CCmpRangeManager::LosIsOffWorld needs to be kept in sync with this const int edgeSize = 3 * Pathfinding::NAVCELLS_PER_TILE; // number of tiles around the edge that will be off-world NavcellData edgeMask = 0; for (PathfinderPassability& passability : m_PassClasses) edgeMask |= passability.m_Mask; int w = m_TerrainOnlyGrid->m_W; int h = m_TerrainOnlyGrid->m_H; if (cmpObstructionManager->GetPassabilityCircular()) { for (int j = 0; j < h; ++j) { for (int i = 0; i < w; ++i) { // Based on CCmpRangeManager::LosIsOffWorld // but tweaked since it's tile-based instead. // (We double all the values so we can handle half-tile coordinates.) // This needs to be slightly tighter than the LOS circle, // else units might get themselves lost in the SoD around the edge. int dist2 = (i*2 + 1 - w)*(i*2 + 1 - w) + (j*2 + 1 - h)*(j*2 + 1 - h); if (dist2 >= (w - 2*edgeSize) * (h - 2*edgeSize)) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); } } } else { for (u16 j = 0; j < h; ++j) for (u16 i = 0; i < edgeSize; ++i) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); for (u16 j = 0; j < h; ++j) for (u16 i = w-edgeSize+1; i < w; ++i) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); for (u16 j = 0; j < edgeSize; ++j) for (u16 i = edgeSize; i < w-edgeSize+1; ++i) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); for (u16 j = h-edgeSize+1; j < h; ++j) for (u16 i = edgeSize; i < w-edgeSize+1; ++i) m_TerrainOnlyGrid->set(i, j, m_TerrainOnlyGrid->get(i, j) | edgeMask); } if (!expandPassability) return; // Expand the impassability grid, for any class with non-zero clearance, // so that we can stop units getting too close to impassable navcells. // Note: It's not possible to perform this expansion once for all passabilities // with the same clearance, because the impassable cells are not necessarily the // same for all these passabilities. for (PathfinderPassability& passability : m_PassClasses) { if (passability.m_Clearance == fixed::Zero()) continue; int clearance = (passability.m_Clearance / Pathfinding::NAVCELL_SIZE).ToInt_RoundToInfinity(); ExpandImpassableCells(*m_TerrainOnlyGrid, clearance, passability.m_Mask); } } ////////////////////////////////////////////////////////// + +void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) const +{ + m_LongPathfinder->ComputePath(*m_PathfinderHier, x0, z0, goal, passClass, ret); +} + u32 CCmpPathfinder::ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) { AsyncLongPathRequest req = { m_NextAsyncTicket++, x0, z0, goal, passClass, notify }; m_AsyncLongPathRequests.push_back(req); return req.ticket; } u32 CCmpPathfinder::ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t group, entity_id_t notify) { AsyncShortPathRequest req = { m_NextAsyncTicket++, x0, z0, clearance, range, goal, passClass, avoidMovingUnits, group, notify }; m_AsyncShortPathRequests.push_back(req); return req.ticket; } WaypointPath CCmpPathfinder::ComputeShortPath(const AsyncShortPathRequest& request) const { return m_VertexPathfinder->ComputeShortPath(request, CmpPtr(GetSystemEntity())); } // Async processing: void CCmpPathfinder::FinishAsyncRequests() { PROFILE2("Finish Async Requests"); // Save the request queue in case it gets modified while iterating std::vector longRequests; m_AsyncLongPathRequests.swap(longRequests); std::vector shortRequests; m_AsyncShortPathRequests.swap(shortRequests); // TODO: we should only compute one path per entity per turn // TODO: this computation should be done incrementally, spread // across multiple frames (or even multiple turns) ProcessLongRequests(longRequests); ProcessShortRequests(shortRequests); } void CCmpPathfinder::ProcessLongRequests(const std::vector& longRequests) { PROFILE2("Process Long Requests"); for (size_t i = 0; i < longRequests.size(); ++i) { const AsyncLongPathRequest& req = longRequests[i]; WaypointPath path; ComputePath(req.x0, req.z0, req.goal, req.passClass, path); CMessagePathResult msg(req.ticket, path); GetSimContext().GetComponentManager().PostMessage(req.notify, msg); } } void CCmpPathfinder::ProcessShortRequests(const std::vector& shortRequests) { PROFILE2("Process Short Requests"); for (size_t i = 0; i < shortRequests.size(); ++i) { const AsyncShortPathRequest& req = shortRequests[i]; WaypointPath path = m_VertexPathfinder->ComputeShortPath(req, CmpPtr(GetSystemEntity())); CMessagePathResult msg(req.ticket, path); GetSimContext().GetComponentManager().PostMessage(req.notify, msg); } } void CCmpPathfinder::ProcessSameTurnMoves() { if (!m_AsyncLongPathRequests.empty()) { // Figure out how many moves we can do this time i32 moveCount = m_MaxSameTurnMoves - m_SameTurnMovesCount; if (moveCount <= 0) return; // Copy the long request elements we are going to process into a new array std::vector longRequests; if ((i32)m_AsyncLongPathRequests.size() <= moveCount) { m_AsyncLongPathRequests.swap(longRequests); moveCount = (i32)longRequests.size(); } else { longRequests.resize(moveCount); copy(m_AsyncLongPathRequests.begin(), m_AsyncLongPathRequests.begin() + moveCount, longRequests.begin()); m_AsyncLongPathRequests.erase(m_AsyncLongPathRequests.begin(), m_AsyncLongPathRequests.begin() + moveCount); } ProcessLongRequests(longRequests); m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); } if (!m_AsyncShortPathRequests.empty()) { // Figure out how many moves we can do now i32 moveCount = m_MaxSameTurnMoves - m_SameTurnMovesCount; if (moveCount <= 0) return; // Copy the short request elements we are going to process into a new array std::vector shortRequests; if ((i32)m_AsyncShortPathRequests.size() <= moveCount) { m_AsyncShortPathRequests.swap(shortRequests); moveCount = (i32)shortRequests.size(); } else { shortRequests.resize(moveCount); copy(m_AsyncShortPathRequests.begin(), m_AsyncShortPathRequests.begin() + moveCount, shortRequests.begin()); m_AsyncShortPathRequests.erase(m_AsyncShortPathRequests.begin(), m_AsyncShortPathRequests.begin() + moveCount); } ProcessShortRequests(shortRequests); m_SameTurnMovesCount = (u16)(m_SameTurnMovesCount + moveCount); } } ////////////////////////////////////////////////////////// bool CCmpPathfinder::CheckMovement(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, pass_class_t passClass) const { PROFILE2_IFSPIKE("Check Movement", 0.001); // Test against obstructions first. filter may discard pathfinding-blocking obstructions. // Use more permissive version of TestLine to allow unit-unit collisions to overlap slightly. // This makes movement smoother and more natural for units, overall. CmpPtr cmpObstructionManager(GetSystemEntity()); if (!cmpObstructionManager || cmpObstructionManager->TestLine(filter, x0, z0, x1, z1, r, true)) return false; // Then test against the terrain grid. This should not be necessary // But in case we allow terrain to change it will become so. return Pathfinding::CheckLineMovement(x0, z0, x1, z1, passClass, *m_TerrainOnlyGrid); } ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool UNUSED(onlyCenterPoint)) const { // Check unit obstruction CmpPtr cmpObstructionManager(GetSystemEntity()); if (!cmpObstructionManager) return ICmpObstruction::FOUNDATION_CHECK_FAIL_ERROR; if (cmpObstructionManager->TestUnitShape(filter, x, z, r, NULL)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_OBSTRUCTS_FOUNDATION; // Test against terrain and static obstructions: u16 i, j; Pathfinding::NearestNavcell(x, z, i, j, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); if (!IS_PASSABLE(m_Grid->get(i, j), passClass)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; // (Static obstructions will be redundantly tested against in both the // obstruction-shape test and navcell-passability test, which is slightly // inefficient but shouldn't affect behaviour) return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; } ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckBuildingPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, entity_id_t id, pass_class_t passClass) const { return CCmpPathfinder::CheckBuildingPlacement(filter, x, z, a, w, h, id, passClass, false); } ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckBuildingPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, entity_id_t id, pass_class_t passClass, bool UNUSED(onlyCenterPoint)) const { // Check unit obstruction CmpPtr cmpObstructionManager(GetSystemEntity()); if (!cmpObstructionManager) return ICmpObstruction::FOUNDATION_CHECK_FAIL_ERROR; if (cmpObstructionManager->TestStaticShape(filter, x, z, a, w, h, NULL)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_OBSTRUCTS_FOUNDATION; // Test against terrain: ICmpObstructionManager::ObstructionSquare square; CmpPtr cmpObstruction(GetSimContext(), id); if (!cmpObstruction || !cmpObstruction->GetObstructionSquare(square)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_NO_OBSTRUCTION; entity_pos_t expand; const PathfinderPassability* passability = GetPassabilityFromMask(passClass); if (passability) expand = passability->m_Clearance; SimRasterize::Spans spans; SimRasterize::RasterizeRectWithClearance(spans, square, expand, Pathfinding::NAVCELL_SIZE); for (const SimRasterize::Span& span : spans) { i16 i0 = span.i0; i16 i1 = span.i1; i16 j = span.j; // Fail if any span extends outside the grid if (i0 < 0 || i1 > m_TerrainOnlyGrid->m_W || j < 0 || j > m_TerrainOnlyGrid->m_H) return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; // Fail if any span includes an impassable tile for (i16 i = i0; i < i1; ++i) if (!IS_PASSABLE(m_TerrainOnlyGrid->get(i, j), passClass)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; } return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; } Index: ps/trunk/source/simulation2/components/CCmpPathfinder_Common.h =================================================================== --- ps/trunk/source/simulation2/components/CCmpPathfinder_Common.h (revision 22277) +++ ps/trunk/source/simulation2/components/CCmpPathfinder_Common.h (revision 22278) @@ -1,258 +1,257 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #ifndef INCLUDED_CCMPPATHFINDER_COMMON #define INCLUDED_CCMPPATHFINDER_COMMON /** * @file * Declares CCmpPathfinder. Its implementation is mainly done in CCmpPathfinder.cpp, * but the short-range (vertex) pathfinding is done in CCmpPathfinder_Vertex.cpp. * This file provides common code needed for both files. * * The long-range pathfinding is done by a LongPathfinder object. */ #include "simulation2/system/Component.h" #include "ICmpPathfinder.h" #include "graphics/Overlay.h" #include "graphics/Terrain.h" #include "maths/MathUtil.h" #include "ps/CLogger.h" #include "renderer/TerrainOverlay.h" #include "simulation2/components/ICmpObstructionManager.h" -#include "simulation2/helpers/LongPathfinder.h" + +class HierarchicalPathfinder; +class LongPathfinder; class VertexPathfinder; class SceneCollector; class AtlasOverlay; #ifdef NDEBUG #define PATHFIND_DEBUG 0 #else #define PATHFIND_DEBUG 1 #endif /** * Implementation of ICmpPathfinder */ class CCmpPathfinder final : public ICmpPathfinder { public: static void ClassInit(CComponentManager& componentManager) { componentManager.SubscribeToMessageType(MT_Update); componentManager.SubscribeToMessageType(MT_RenderSubmit); // for debug overlays componentManager.SubscribeToMessageType(MT_TerrainChanged); componentManager.SubscribeToMessageType(MT_WaterChanged); componentManager.SubscribeToMessageType(MT_ObstructionMapShapeChanged); componentManager.SubscribeToMessageType(MT_TurnStart); } ~CCmpPathfinder(); DEFAULT_COMPONENT_ALLOCATOR(Pathfinder) // Template state: std::map m_PassClassMasks; std::vector m_PassClasses; // Dynamic state: std::vector m_AsyncLongPathRequests; std::vector m_AsyncShortPathRequests; u32 m_NextAsyncTicket; // unique IDs for asynchronous path requests u16 m_SameTurnMovesCount; // current number of same turn moves we have processed this turn // Lazily-constructed dynamic state (not serialized): u16 m_MapSize; // tiles per side Grid* m_Grid; // terrain/passability information Grid* m_TerrainOnlyGrid; // same as m_Grid, but only with terrain, to avoid some recomputations // Keep clever updates in memory to avoid memory fragmentation from the grid. // This should be used only in UpdateGrid(), there is no guarantee the data is properly initialized anywhere else. GridUpdateInformation m_DirtinessInformation; // The data from clever updates is stored for the AI manager GridUpdateInformation m_AIPathfinderDirtinessInformation; bool m_TerrainDirty; std::unique_ptr m_VertexPathfinder; - // Interface to the long-range pathfinder. - LongPathfinder m_LongPathfinder; + std::unique_ptr m_PathfinderHier; + std::unique_ptr m_LongPathfinder; // For responsiveness we will process some moves in the same turn they were generated in u16 m_MaxSameTurnMoves; // max number of moves that can be created and processed in the same turn AtlasOverlay* m_AtlasOverlay; static std::string GetSchema() { return ""; } virtual void Init(const CParamNode& paramNode); virtual void Deinit(); template void SerializeCommon(S& serialize); virtual void Serialize(ISerializer& serialize); virtual void Deserialize(const CParamNode& paramNode, IDeserializer& deserialize); virtual void HandleMessage(const CMessage& msg, bool global); virtual pass_class_t GetPassabilityClass(const std::string& name) const; virtual void GetPassabilityClasses(std::map& passClasses) const; virtual void GetPassabilityClasses( std::map& nonPathfindingPassClasses, std::map& pathfindingPassClasses) const; const PathfinderPassability* GetPassabilityFromMask(pass_class_t passClass) const; virtual entity_pos_t GetClearance(pass_class_t passClass) const { const PathfinderPassability* passability = GetPassabilityFromMask(passClass); if (!passability) return fixed::Zero(); return passability->m_Clearance; } virtual entity_pos_t GetMaximumClearance() const { entity_pos_t max = fixed::Zero(); for (const PathfinderPassability& passability : m_PassClasses) if (passability.m_Clearance > max) max = passability.m_Clearance; return max + Pathfinding::CLEARANCE_EXTENSION_RADIUS; } virtual const Grid& GetPassabilityGrid(); virtual const GridUpdateInformation& GetAIPathfinderDirtinessInformation() const { return m_AIPathfinderDirtinessInformation; } virtual void FlushAIPathfinderDirtinessInformation() { m_AIPathfinderDirtinessInformation.Clean(); } virtual Grid ComputeShoreGrid(bool expandOnWater = false); - virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) - { - m_LongPathfinder.ComputePath(x0, z0, goal, passClass, ret); - } + virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) const; virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify); virtual WaypointPath ComputeShortPath(const AsyncShortPathRequest& request) const; virtual u32 ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t controller, entity_id_t notify); virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass); virtual void SetDebugOverlay(bool enabled); virtual void SetHierDebugOverlay(bool enabled); virtual void GetDebugData(u32& steps, double& time, Grid& grid) const; virtual void SetAtlasOverlay(bool enable, pass_class_t passClass = 0); virtual bool CheckMovement(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, pass_class_t passClass) const; virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint) const; virtual ICmpObstruction::EFoundationCheck CheckBuildingPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, entity_id_t id, pass_class_t passClass) const; virtual ICmpObstruction::EFoundationCheck CheckBuildingPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, entity_id_t id, pass_class_t passClass, bool onlyCenterPoint) const; virtual void FinishAsyncRequests(); void ProcessLongRequests(const std::vector& longRequests); void ProcessShortRequests(const std::vector& shortRequests); virtual void ProcessSameTurnMoves(); /** * Regenerates the grid based on the current obstruction list, if necessary */ virtual void UpdateGrid(); /** * Updates the terrain-only grid without updating the dirtiness informations. * Useful for fast passability updates in Atlas. */ void MinimalTerrainUpdate(); /** * Regenerates the terrain-only grid. * Atlas doesn't need to have passability cells expanded. */ void TerrainUpdateHelper(bool expandPassability = true); void RenderSubmit(SceneCollector& collector); }; class AtlasOverlay : public TerrainTextureOverlay { public: const CCmpPathfinder* m_Pathfinder; pass_class_t m_PassClass; AtlasOverlay(const CCmpPathfinder* pathfinder, pass_class_t passClass) : TerrainTextureOverlay(Pathfinding::NAVCELLS_PER_TILE), m_Pathfinder(pathfinder), m_PassClass(passClass) { } virtual void BuildTextureRGBA(u8* data, size_t w, size_t h) { // Render navcell passability, based on the terrain-only grid u8* p = data; for (size_t j = 0; j < h; ++j) { for (size_t i = 0; i < w; ++i) { SColor4ub color(0, 0, 0, 0); if (!IS_PASSABLE(m_Pathfinder->m_TerrainOnlyGrid->get((int)i, (int)j), m_PassClass)) color = SColor4ub(255, 0, 0, 127); *p++ = color.R; *p++ = color.G; *p++ = color.B; *p++ = color.A; } } } }; #endif // INCLUDED_CCMPPATHFINDER_COMMON Index: ps/trunk/source/simulation2/components/ICmpPathfinder.h =================================================================== --- ps/trunk/source/simulation2/components/ICmpPathfinder.h (revision 22277) +++ ps/trunk/source/simulation2/components/ICmpPathfinder.h (revision 22278) @@ -1,199 +1,199 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #ifndef INCLUDED_ICMPPATHFINDER #define INCLUDED_ICMPPATHFINDER #include "simulation2/system/Interface.h" #include "simulation2/components/ICmpObstruction.h" #include "simulation2/helpers/PathGoal.h" #include "simulation2/helpers/Pathfinding.h" #include "maths/FixedVector2D.h" #include #include class IObstructionTestFilter; template class Grid; /** * Pathfinder algorithms. * * There are two different modes: a tile-based pathfinder that works over long distances and * accounts for terrain costs but ignore units, and a 'short' vertex-based pathfinder that * provides precise paths and avoids other units. * * Both use the same concept of a PathGoal: either a point, circle or square. * (If the starting point is inside the goal shape then the path will move outwards * to reach the shape's outline.) * * The output is a list of waypoints. */ class ICmpPathfinder : public IComponent { public: /** * Get the list of all known passability classes. */ virtual void GetPassabilityClasses(std::map& passClasses) const = 0; /** * Get the list of passability classes, separating pathfinding classes and others. */ virtual void GetPassabilityClasses( std::map& nonPathfindingPassClasses, std::map& pathfindingPassClasses) const = 0; /** * Get the tag for a given passability class name. * Logs an error and returns something acceptable if the name is unrecognised. */ virtual pass_class_t GetPassabilityClass(const std::string& name) const = 0; virtual entity_pos_t GetClearance(pass_class_t passClass) const = 0; /** * Get the larger clearance in all passability classes. */ virtual entity_pos_t GetMaximumClearance() const = 0; virtual const Grid& GetPassabilityGrid() = 0; /** * Get the accumulated dirtiness information since the last time the AI accessed and flushed it. */ virtual const GridUpdateInformation& GetAIPathfinderDirtinessInformation() const = 0; virtual void FlushAIPathfinderDirtinessInformation() = 0; /** * Get a grid representing the distance to the shore of the terrain tile. */ virtual Grid ComputeShoreGrid(bool expandOnWater = false) = 0; /** * Compute a tile-based path from the given point to the goal, and return the set of waypoints. * The waypoints correspond to the centers of horizontally/vertically adjacent tiles * along the path. */ - virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) = 0; + virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) const = 0; /** * Asynchronous version of ComputePath. * The result will be sent as CMessagePathResult to 'notify'. * Returns a unique non-zero number, which will match the 'ticket' in the result, * so callers can recognise each individual request they make. */ virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) = 0; /** * If the debug overlay is enabled, render the path that will computed by ComputePath. */ virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) = 0; /** * Compute a precise path from the given point to the goal, and return the set of waypoints. * The path is based on the full set of obstructions that pass the filter, such that * a unit of clearance 'clearance' will be able to follow the path with no collisions. * The path is restricted to a box of radius 'range' from the starting point. */ virtual WaypointPath ComputeShortPath(const AsyncShortPathRequest& request) const = 0; /** * Asynchronous version of ComputeShortPath (using ControlGroupObstructionFilter). * The result will be sent as CMessagePathResult to 'notify'. * Returns a unique non-zero number, which will match the 'ticket' in the result, * so callers can recognise each individual request they make. */ virtual u32 ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t group, entity_id_t notify) = 0; /** * Check whether the given movement line is valid and doesn't hit any obstructions * or impassable terrain. * Returns true if the movement is okay. */ virtual bool CheckMovement(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, pass_class_t passClass) const = 0; /** * Check whether a unit placed here is valid and doesn't hit any obstructions * or impassable terrain. * When onlyCenterPoint = true, only check the center tile of the unit * @return ICmpObstruction::FOUNDATION_CHECK_SUCCESS if the placement is okay, else * a value describing the type of failure. */ virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint = false) const = 0; /** * Check whether a building placed here is valid and doesn't hit any obstructions * or impassable terrain. * @return ICmpObstruction::FOUNDATION_CHECK_SUCCESS if the placement is okay, else * a value describing the type of failure. */ virtual ICmpObstruction::EFoundationCheck CheckBuildingPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, entity_id_t id, pass_class_t passClass) const = 0; /** * Check whether a building placed here is valid and doesn't hit any obstructions * or impassable terrain. * when onlyCenterPoint = true, only check the center tile of the building * @return ICmpObstruction::FOUNDATION_CHECK_SUCCESS if the placement is okay, else * a value describing the type of failure. */ virtual ICmpObstruction::EFoundationCheck CheckBuildingPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, entity_id_t id, pass_class_t passClass, bool onlyCenterPoint) const = 0; /** * Toggle the storage and rendering of debug info. */ virtual void SetDebugOverlay(bool enabled) = 0; /** * Toggle the storage and rendering of debug info for the hierarchical pathfinder. */ virtual void SetHierDebugOverlay(bool enabled) = 0; /** * Finish computing asynchronous path requests and send the CMessagePathResult messages. */ virtual void FinishAsyncRequests() = 0; /** * Process moves during the same turn they were created in to improve responsiveness. */ virtual void ProcessSameTurnMoves() = 0; /** * Regenerates the grid based on the current obstruction list, if necessary */ virtual void UpdateGrid() = 0; /** * Returns some stats about the last ComputePath. */ virtual void GetDebugData(u32& steps, double& time, Grid& grid) const = 0; /** * Sets up the pathfinder passability overlay in Atlas. */ virtual void SetAtlasOverlay(bool enable, pass_class_t passClass = 0) = 0; DECLARE_INTERFACE_TYPE(Pathfinder) }; #endif // INCLUDED_ICMPPATHFINDER Index: ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp =================================================================== --- ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp (revision 22277) +++ ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp (revision 22278) @@ -1,777 +1,787 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #include "precompiled.h" #include "HierarchicalPathfinder.h" #include "graphics/Overlay.h" #include "ps/Profile.h" +#include "renderer/Scene.h" // Find the root ID of a region, used by InitRegions inline u16 RootID(u16 x, const std::vector& v) { while (v[x] < x) x = v[x]; return x; } void HierarchicalPathfinder::Chunk::InitRegions(int ci, int cj, Grid* grid, pass_class_t passClass) { ENSURE(ci < 256 && cj < 256); // avoid overflows m_ChunkI = ci; m_ChunkJ = cj; memset(m_Regions, 0, sizeof(m_Regions)); int i0 = ci * CHUNK_SIZE; int j0 = cj * CHUNK_SIZE; int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W); int j1 = std::min(j0 + CHUNK_SIZE, (int)grid->m_H); // Efficiently flood-fill the m_Regions grid int regionID = 0; std::vector connect; u16* pCurrentID = NULL; u16 LeftID = 0; u16 DownID = 0; bool Checked = false; // prevent some unneccessary RootID calls connect.reserve(32); // TODO: What's a sensible number? connect.push_back(0); // connect[0] = 0 // Start by filling the grid with 0 for blocked, // and regionID for unblocked for (int j = j0; j < j1; ++j) { for (int i = i0; i < i1; ++i) { pCurrentID = &m_Regions[j-j0][i-i0]; if (!IS_PASSABLE(grid->get(i, j), passClass)) { *pCurrentID = 0; continue; } if (j > j0) DownID = m_Regions[j-1-j0][i-i0]; if (i == i0) LeftID = 0; else LeftID = m_Regions[j-j0][i-1-i0]; if (LeftID > 0) { *pCurrentID = LeftID; if (*pCurrentID != DownID && DownID > 0 && !Checked) { u16 id0 = RootID(DownID, connect); u16 id1 = RootID(LeftID, connect); Checked = true; // this avoids repeatedly connecting the same IDs if (id0 < id1) connect[id1] = id0; else if (id0 > id1) connect[id0] = id1; } else if (DownID == 0) Checked = false; } else if (DownID > 0) { *pCurrentID = DownID; Checked = false; } else { // New ID *pCurrentID = ++regionID; connect.push_back(regionID); Checked = false; } } } // Mark connected regions as being the same ID (i.e. the lowest) m_RegionsID.clear(); for (u16 i = 1; i < regionID+1; ++i) { if (connect[i] != i) connect[i] = RootID(i, connect); else m_RegionsID.push_back(connect[i]); } // Replace IDs by the root ID for (int j = 0; j < CHUNK_SIZE; ++j) for (int i = 0; i < CHUNK_SIZE; ++i) m_Regions[j][i] = connect[m_Regions[j][i]]; } /** * Returns a RegionID for the given global navcell coords * (which must be inside this chunk); */ HierarchicalPathfinder::RegionID HierarchicalPathfinder::Chunk::Get(int i, int j) const { ENSURE(i < CHUNK_SIZE && j < CHUNK_SIZE); return RegionID(m_ChunkI, m_ChunkJ, m_Regions[j][i]); } /** * Return the global navcell coords that correspond roughly to the * center of the given region in this chunk. * (This is not guaranteed to be actually inside the region.) */ void HierarchicalPathfinder::Chunk::RegionCenter(u16 r, int& i_out, int& j_out) const { // Find the mean of i,j coords of navcells in this region: u32 si = 0, sj = 0; // sum of i,j coords u32 n = 0; // number of navcells in region cassert(CHUNK_SIZE < 256); // conservative limit to ensure si and sj don't overflow for (int j = 0; j < CHUNK_SIZE; ++j) { for (int i = 0; i < CHUNK_SIZE; ++i) { if (m_Regions[j][i] == r) { si += i; sj += j; n += 1; } } } // Avoid divide-by-zero if (n == 0) n = 1; i_out = m_ChunkI * CHUNK_SIZE + si / n; j_out = m_ChunkJ * CHUNK_SIZE + sj / n; } /** * 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. */ void HierarchicalPathfinder::Chunk::RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const { iBest = 0; jBest = 0; dist2Best = std::numeric_limits::max(); for (int j = 0; j < CHUNK_SIZE; ++j) { for (int i = 0; i < CHUNK_SIZE; ++i) { if (m_Regions[j][i] != r) continue; 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; } } } } /** * Gives the global navcell coords, and the squared distance to the (i0,j0) * navcell, of whichever navcell inside the given region and inside the given goal * is closest to (i0,j0) * Returns true if the goal is inside the region, false otherwise. */ bool HierarchicalPathfinder::Chunk::RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const { // TODO: this should be optimized further. // Most used cases empirically seem to be SQUARE, INVERTED_CIRCLE and then POINT and CIRCLE somehwat equally iOut = 0; jOut = 0; dist2Best = std::numeric_limits::max(); // Calculate the navcell that contains the center of the goal. int gi = (goal.x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity(); int gj = (goal.z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity(); switch(goal.type) { case PathGoal::POINT: { // i and j can be equal to CHUNK_SIZE on the top and right borders of the map, // specially when mapSize is a multiple of CHUNK_SIZE int i = std::min((int)CHUNK_SIZE - 1, gi - m_ChunkI * CHUNK_SIZE); int j = std::min((int)CHUNK_SIZE - 1, gj - m_ChunkJ * CHUNK_SIZE); if (m_Regions[j][i] == r) { iOut = gi; jOut = gj; dist2Best = (gi - i0)*(gi - i0) + (gj - j0)*(gj - j0); return true; } return false; } case PathGoal::CIRCLE: case PathGoal::SQUARE: { // restrict ourselves to a square surrounding the goal. int radius = (std::max(goal.hw*3/2,goal.hh*3/2) >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToInfinity(); int imin = std::max(0, gi-m_ChunkI*CHUNK_SIZE-radius); int imax = std::min((int)CHUNK_SIZE, gi-m_ChunkI*CHUNK_SIZE+radius+1); int jmin = std::max(0, gj-m_ChunkJ*CHUNK_SIZE-radius); int jmax = std::min((int)CHUNK_SIZE, gj-m_ChunkJ*CHUNK_SIZE+radius+1); bool found = false; u32 dist2 = std::numeric_limits::max(); for (u16 j = jmin; j < jmax; ++j) { for (u16 i = imin; i < imax; ++i) { if (m_Regions[j][i] != r) continue; if (found) { dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0) + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0); if (dist2 >= dist2Best) continue; } if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j)) { if (!found) { found = true; dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0) + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0); } iOut = i + m_ChunkI*CHUNK_SIZE; jOut = j + m_ChunkJ*CHUNK_SIZE; dist2Best = dist2; } } } return found; } case PathGoal::INVERTED_CIRCLE: case PathGoal::INVERTED_SQUARE: { bool found = false; u32 dist2 = std::numeric_limits::max(); // loop over all navcells. for (u16 j = 0; j < CHUNK_SIZE; ++j) { for (u16 i = 0; i < CHUNK_SIZE; ++i) { if (m_Regions[j][i] != r) continue; if (found) { dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0) + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0); if (dist2 >= dist2Best) continue; } if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j)) { if (!found) { found = true; dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0) + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0); } iOut = i + m_ChunkI*CHUNK_SIZE; jOut = j + m_ChunkJ*CHUNK_SIZE; dist2Best = dist2; } } } return found; } } return false; } HierarchicalPathfinder::HierarchicalPathfinder() : m_DebugOverlay(NULL) { } HierarchicalPathfinder::~HierarchicalPathfinder() { SAFE_DELETE(m_DebugOverlay); } void HierarchicalPathfinder::SetDebugOverlay(bool enabled, const CSimContext* simContext) { if (enabled && !m_DebugOverlay) { m_DebugOverlay = new HierarchicalOverlay(*this); m_DebugOverlayLines.clear(); m_SimContext = simContext; AddDebugEdges(GetPassabilityClass("default")); } else if (!enabled && m_DebugOverlay) { SAFE_DELETE(m_DebugOverlay); m_DebugOverlayLines.clear(); m_SimContext = NULL; } } +void HierarchicalPathfinder::RenderSubmit(SceneCollector& collector) +{ + if (!m_DebugOverlay) + return; + + for (size_t i = 0; i < m_DebugOverlayLines.size(); ++i) + collector.Submit(&m_DebugOverlayLines[i]); +} + void HierarchicalPathfinder::Recompute(Grid* grid, const std::map& nonPathfindingPassClassMasks, const std::map& pathfindingPassClassMasks) { PROFILE2("Hierarchical Recompute"); m_PassClassMasks = pathfindingPassClassMasks; std::map allPassClasses = m_PassClassMasks; allPassClasses.insert(nonPathfindingPassClassMasks.begin(), nonPathfindingPassClassMasks.end()); 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; m_Chunks.clear(); m_Edges.clear(); for (auto& passClassMask : allPassClasses) { pass_class_t passClass = passClassMask.second; // Compute the regions within each chunk m_Chunks[passClass].resize(m_ChunksW*m_ChunksH); for (int cj = 0; cj < m_ChunksH; ++cj) { for (int ci = 0; ci < m_ChunksW; ++ci) { m_Chunks[passClass].at(cj*m_ChunksW + ci).InitRegions(ci, cj, grid, passClass); } } // Construct the search graph over the regions EdgesMap& edges = m_Edges[passClass]; for (int cj = 0; cj < m_ChunksH; ++cj) { for (int ci = 0; ci < m_ChunksW; ++ci) { FindEdges(ci, cj, passClass, edges); } } } if (m_DebugOverlay) { m_DebugOverlayLines.clear(); AddDebugEdges(GetPassabilityClass("default")); } } void HierarchicalPathfinder::Update(Grid* grid, const Grid& dirtinessGrid) { PROFILE3("Hierarchical Update"); for (int 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) { // Skip chunks where no navcells are dirty. int i0 = ci * CHUNK_SIZE; int i1 = std::min(i0 + CHUNK_SIZE, (int)dirtinessGrid.m_W); if (!dirtinessGrid.any_set_in_square(i0, j0, i1, j1)) continue; for (const std::pair& passClassMask : m_PassClassMasks) { pass_class_t passClass = passClassMask.second; Chunk& a = m_Chunks[passClass].at(ci + cj*m_ChunksW); 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 (int cj = 0; cj < m_ChunksH; ++cj) { for (int ci = 0; ci < m_ChunksW; ++ci) { FindEdges(ci, cj, passClass, edges); } } } if (m_DebugOverlay) { m_DebugOverlayLines.clear(); AddDebugEdges(GetPassabilityClass("default")); } } /** * Find edges between regions in this chunk and the adjacent below/left chunks. */ void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) { 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)); 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; } } } } /** * Debug visualisation of graph edges between regions. */ void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass) { const EdgesMap& edges = m_Edges[passClass]; const std::vector& chunks = m_Chunks[passClass]; for (auto& edge : edges) { for (const RegionID& region: edge.second) { // Draw a line between the two regions' centers int i0, j0, i1, j1; chunks[edge.first.cj * m_ChunksW + edge.first.ci].RegionCenter(edge.first.r, i0, j0); chunks[region.cj * m_ChunksW + region.ci].RegionCenter(region.r, i1, j1); CFixedVector2D a, b; Pathfinding::NavcellCenter(i0, j0, a.X, a.Y); Pathfinding::NavcellCenter(i1, j1, b.X, b.Y); // Push the endpoints inwards a little to avoid overlaps CFixedVector2D d = b - a; d.Normalize(entity_pos_t::FromInt(1)); a += d; b -= d; std::vector xz; xz.push_back(a.X.ToFloat()); xz.push_back(a.Y.ToFloat()); xz.push_back(b.X.ToFloat()); xz.push_back(b.Y.ToFloat()); m_DebugOverlayLines.emplace_back(); m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0); SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true); } } } 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.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) const { PROFILE2("MakeGoalReachable"); RegionID source = Get(i0, j0, passClass); // Find everywhere that's reachable std::set reachableRegions; FindReachableRegions(source, reachableRegions, passClass); // Check whether any reachable region contains the goal // And get the navcell that's the closest to the point u16 bestI = 0; u16 bestJ = 0; u32 dist2Best = std::numeric_limits::max(); for (const RegionID& region : reachableRegions) { // 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; u16 i,j; u32 dist2; // 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) { bestI = i; bestJ = j; dist2Best = dist2; } } } // 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; return; } ENSURE(dist2Best != std::numeric_limits::max()); PathGoal newGoal; newGoal.type = PathGoal::POINT; Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z); goal = newGoal; } void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const { std::set regions; FindPassableRegions(regions, passClass); FindNearestNavcellInRegions(regions, i, j, passClass); } void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) 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 // * Sort regions by that underestimated distance // * For each region, find the actual nearest navcell // * Stop when the underestimated distances are worse than the best real distance std::vector > regionDistEsts; // pair of (distance^2, region) for (const RegionID& region : regions) { int i0 = region.ci * CHUNK_SIZE; int j0 = region.cj * CHUNK_SIZE; int i1 = i0 + CHUNK_SIZE - 1; int j1 = j0 + CHUNK_SIZE - 1; // Pick the point in the chunk nearest the goal int iNear = Clamp((int)iGoal, i0, i1); int jNear = Clamp((int)jGoal, j0, j1); int dist2 = (iNear - iGoal)*(iNear - iGoal) + (jNear - jGoal)*(jNear - jGoal); regionDistEsts.emplace_back(dist2, region); } // Sort by increasing distance (tie-break on RegionID) std::sort(regionDistEsts.begin(), regionDistEsts.end()); int iBest = iGoal; int jBest = jGoal; u32 dist2Best = std::numeric_limits::max(); for (auto& pair : regionDistEsts) { if (pair.first >= dist2Best) break; RegionID region = pair.second; int i, j; u32 dist2; GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, i, j, dist2); if (dist2 < dist2Best) { iBest = i; jBest = j; dist2Best = dist2; } } iGoal = iBest; jGoal = jBest; } 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; 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::FindPassableRegions(std::set& regions, pass_class_t passClass) const { // Construct a set of all regions of all chunks for this pass class for (const Chunk& chunk : m_Chunks.at(passClass)) for (int r : chunk.m_RegionsID) regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r)); } 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.at(passClass)[region.cj * m_ChunksW + region.ci]; for (int j = 0; j < CHUNK_SIZE; ++j) for (int i = 0; i < CHUNK_SIZE; ++i) if (c.m_Regions[j][i] == region.r) grid.set(i0 + i, j0 + j, value); } Grid HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) const { Grid connectivityGrid(m_W, m_H); connectivityGrid.reset(); u16 idx = 1; for (size_t i = 0; i < m_W; ++i) { for (size_t j = 0; j < m_H; ++j) { if (connectivityGrid.get(i, j) != 0) continue; RegionID from = Get(i, j, passClass); if (from.r == 0) continue; std::set reachable; FindReachableRegions(from, reachable, passClass); for (const RegionID& region : reachable) FillRegionOnGrid(region, passClass, idx, connectivityGrid); ++idx; } } return connectivityGrid; } Index: ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h =================================================================== --- ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h (revision 22277) +++ ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h (revision 22278) @@ -1,240 +1,243 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #ifndef INCLUDED_HIERPATHFINDER #define INCLUDED_HIERPATHFINDER #include "Pathfinding.h" #include "renderer/TerrainOverlay.h" #include "Render.h" #include "graphics/SColor.h" /** * Hierarchical pathfinder. * * 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 * is defined as a region. * 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.) * * 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; class TestHierarchicalPathfinder; #endif class HierarchicalOverlay; +class SceneCollector; class HierarchicalPathfinder { #ifdef TEST friend class TestCmpPathfinder; friend class TestHierarchicalPathfinder; #endif public: struct RegionID { u8 ci, cj; // chunk ID u16 r; // unique-per-chunk local region ID RegionID(u8 ci, u8 cj, u16 r) : ci(ci), cj(cj), r(r) { } bool operator<(const RegionID& b) const { // Sort by chunk ID, then by per-chunk region ID if (ci < b.ci) return true; if (b.ci < ci) return false; if (cj < b.cj) return true; if (b.cj < cj) return false; return r < b.r; } bool operator==(const RegionID& b) const { return ((ci == b.ci) && (cj == b.cj) && (r == b.r)); } }; HierarchicalPathfinder(); ~HierarchicalPathfinder(); void SetDebugOverlay(bool enabled, const CSimContext* simContext); // Non-pathfinding grids will never be recomputed on calling HierarchicalPathfinder::Update void Recompute(Grid* passabilityGrid, const std::map& nonPathfindingPassClassMasks, const std::map& pathfindingPassClassMasks); void Update(Grid* grid, const Grid& dirtinessGrid); RegionID Get(u16 i, u16 j, pass_class_t passClass) const; /** * Updates @p goal so that it's guaranteed to be reachable from the navcell * @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. * * In the case of a non-point reachable goal, it is replaced with a point goal * at the reachable navcell of the goal which is nearest to the starting navcell. */ void MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const; /** * Updates @p i, @p j (which is assumed to be an impassable navcell) * to the nearest passable navcell. */ void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const; /** * Generates the connectivity grid associated with the given pass_class */ Grid GetConnectivityGrid(pass_class_t passClass) const; pass_class_t GetPassabilityClass(const std::string& name) const { auto it = m_PassClassMasks.find(name); if (it != m_PassClassMasks.end()) return it->second; LOGERROR("Invalid passability class name '%s'", name.c_str()); return 0; } + void RenderSubmit(SceneCollector& collector); + private: static const u8 CHUNK_SIZE = 96; // number of navcells per side // TODO: figure out best number. Probably 64 < n < 128 struct Chunk { u8 m_ChunkI, m_ChunkJ; // chunk ID std::vector m_RegionsID; // IDs of local regions, 0 (impassable) excluded u16 m_Regions[CHUNK_SIZE][CHUNK_SIZE]; // local region ID per navcell 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); RegionID Get(int i, int j) const; void RegionCenter(u16 r, int& i, int& j) 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.size() == b.m_RegionsID.size() && 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 FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const; void FindPassableRegions(std::set& regions, pass_class_t passClass) const; /** * Updates @p iGoal and @p jGoal to the navcell that is the nearest to the * initial goal coordinates, in one of the given @p regions. * (Assumes @p regions is non-empty.) */ void FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const; const Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) const { return m_Chunks.at(passClass).at(cj * m_ChunksW + ci); } void FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) const; u16 m_W, m_H; u8 m_ChunksW, m_ChunksH; std::map > m_Chunks; std::map m_Edges; // Passability classes for which grids will be updated when calling Update std::map m_PassClassMasks; void AddDebugEdges(pass_class_t passClass); HierarchicalOverlay* m_DebugOverlay; const CSimContext* m_SimContext; // Used for drawing the debug lines public: std::vector m_DebugOverlayLines; }; class HierarchicalOverlay : public TerrainTextureOverlay { public: HierarchicalPathfinder& m_PathfinderHier; HierarchicalOverlay(HierarchicalPathfinder& pathfinderHier) : TerrainTextureOverlay(Pathfinding::NAVCELLS_PER_TILE), m_PathfinderHier(pathfinderHier) { } virtual void BuildTextureRGBA(u8* data, size_t w, size_t h) { pass_class_t passClass = m_PathfinderHier.GetPassabilityClass("default"); for (size_t j = 0; j < h; ++j) { for (size_t i = 0; i < w; ++i) { SColor4ub color; HierarchicalPathfinder::RegionID rid = m_PathfinderHier.Get(i, j, passClass); if (rid.r == 0) color = SColor4ub(0, 0, 0, 0); else if (rid.r == 0xFFFF) color = SColor4ub(255, 0, 255, 255); else color = GetColor(rid.r + rid.ci*5 + rid.cj*7, 127); *data++ = color.R; *data++ = color.G; *data++ = color.B; *data++ = color.A; } } } }; #endif // INCLUDED_HIERPATHFINDER Index: ps/trunk/source/simulation2/helpers/LongPathfinder.cpp =================================================================== --- ps/trunk/source/simulation2/helpers/LongPathfinder.cpp (revision 22277) +++ ps/trunk/source/simulation2/helpers/LongPathfinder.cpp (revision 22278) @@ -1,1031 +1,1043 @@ /* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #include "precompiled.h" #include "LongPathfinder.h" #include "lib/bits.h" #include "ps/Profile.h" #include "Geometry.h" +#include "HierarchicalPathfinder.h" /** * Jump point cache. * * The JPS algorithm wants to efficiently either find the first jump point * in some direction from some cell (not counting the cell itself), * if it is reachable without crossing any impassable cells; * or know that there is no such reachable jump point. * The jump point is always on a passable cell. * We cache that data to allow fast lookups, which helps performance * significantly (especially on sparse maps). * Recalculation might be expensive but the underlying passability data * changes relatively rarely. * * To allow the algorithm to detect goal cells, we want to treat them as * jump points too. (That means the algorithm will push those cells onto * its open queue, and will eventually pop a goal cell and realise it's done.) * (Goals might be circles/squares/etc, not just a single cell.) * But the goal generally changes for every path request, so we can't cache * it like the normal jump points. * Instead, if there's no jump point from some cell then we'll cache the * first impassable cell as an 'obstruction jump point' * (with a flag to distinguish from a real jump point), and then the caller * can test whether the goal includes a cell that's closer than the first * (obstruction or real) jump point, * and treat the goal cell as a jump point in that case. * * We only ever need to find the jump point relative to a passable cell; * the cache is allowed to return bogus values for impassable cells. */ class JumpPointCache { /** * Simple space-inefficient row storage. */ struct RowRaw { std::vector data; size_t GetMemoryUsage() const { return data.capacity() * sizeof(u16); } RowRaw(int length) { data.resize(length); } /** * Set cells x0 <= x < x1 to have jump point x1. */ void SetRange(int x0, int x1, bool obstruction) { ENSURE(0 <= x0 && x0 <= x1 && x1 < (int)data.size()); for (int x = x0; x < x1; ++x) data[x] = (x1 << 1) | (obstruction ? 1 : 0); } /** * Returns the coordinate of the next jump point xp (where x < xp), * and whether it's an obstruction point or jump point. */ void Get(int x, int& xp, bool& obstruction) { ENSURE(0 <= x && x < (int)data.size()); xp = data[x] >> 1; obstruction = data[x] & 1; } void Finish() { } }; struct RowTree { /** * Represents an interval [u15 x0, u16 x1) * with a boolean obstruction flag, * packed into a single u32. */ struct Interval { Interval() : data(0) { } Interval(int x0, int x1, bool obstruction) { ENSURE(0 <= x0 && x0 < 0x8000); ENSURE(0 <= x1 && x1 < 0x10000); data = ((u32)x0 << 17) | (u32)(obstruction ? 0x10000 : 0) | (u32)x1; } int x0() { return data >> 17; } int x1() { return data & 0xFFFF; } bool obstruction() { return (data & 0x10000) != 0; } u32 data; }; std::vector data; size_t GetMemoryUsage() const { return data.capacity() * sizeof(Interval); } RowTree(int UNUSED(length)) { } void SetRange(int x0, int x1, bool obstruction) { ENSURE(0 <= x0 && x0 <= x1); data.emplace_back(x0, x1, obstruction); } /** * Recursive helper function for Finish(). * Given two ranges [x0, pivot) and [pivot, x1) in the sorted array 'data', * the pivot element is added onto the binary tree (stored flattened in an * array), and then each range is split into two sub-ranges with a pivot in * the middle (to ensure the tree remains balanced) and ConstructTree recurses. */ void ConstructTree(std::vector& tree, size_t x0, size_t pivot, size_t x1, size_t idx_tree) { ENSURE(x0 < data.size()); ENSURE(x1 <= data.size()); ENSURE(x0 <= pivot); ENSURE(pivot < x1); ENSURE(idx_tree < tree.size()); tree[idx_tree] = data[pivot]; if (x0 < pivot) ConstructTree(tree, x0, (x0 + pivot) / 2, pivot, (idx_tree << 1) + 1); if (pivot + 1 < x1) ConstructTree(tree, pivot + 1, (pivot + x1) / 2, x1, (idx_tree << 1) + 2); } void Finish() { // Convert the sorted interval list into a balanced binary tree std::vector tree; if (!data.empty()) { size_t depth = ceil_log2(data.size() + 1); tree.resize((1 << depth) - 1); ConstructTree(tree, 0, data.size() / 2, data.size(), 0); } data.swap(tree); } void Get(int x, int& xp, bool& obstruction) { // Search the binary tree for an interval which contains x int i = 0; while (true) { ENSURE(i < (int)data.size()); Interval interval = data[i]; if (x < interval.x0()) i = (i << 1) + 1; else if (x >= interval.x1()) i = (i << 1) + 2; else { ENSURE(interval.x0() <= x && x < interval.x1()); xp = interval.x1(); obstruction = interval.obstruction(); return; } } } }; // Pick one of the row implementations typedef RowRaw Row; public: int m_Width; int m_Height; std::vector m_JumpPointsRight; std::vector m_JumpPointsLeft; std::vector m_JumpPointsUp; std::vector m_JumpPointsDown; /** * Compute the cached obstruction/jump points for each cell, * in a single direction. By default the code assumes the rightwards * (+i) direction; set 'transpose' to switch to upwards (+j), * and/or set 'mirror' to reverse the direction. */ void ComputeRows(std::vector& rows, const Grid& terrain, pass_class_t passClass, bool transpose, bool mirror) { int w = terrain.m_W; int h = terrain.m_H; if (transpose) std::swap(w, h); // Check the terrain passability, adjusted for transpose/mirror #define TERRAIN_IS_PASSABLE(i, j) \ IS_PASSABLE( \ mirror \ ? (transpose ? terrain.get((j), w-1-(i)) : terrain.get(w-1-(i), (j))) \ : (transpose ? terrain.get((j), (i)) : terrain.get((i), (j))) \ , passClass) rows.reserve(h); for (int j = 0; j < h; ++j) rows.emplace_back(w); for (int j = 1; j < h - 1; ++j) { // Find the first passable cell. // Then, find the next jump/obstruction point after that cell, // and store that point for the passable range up to that cell, // then repeat. int i = 0; while (i < w) { // Restart the 'while' loop until we reach a passable cell if (!TERRAIN_IS_PASSABLE(i, j)) { ++i; continue; } // i is now a passable cell; find the next jump/obstruction point. // (We assume the map is surrounded by impassable cells, so we don't // need to explicitly check for world bounds here.) int i0 = i; while (true) { ++i; // Check if we hit an obstructed tile if (!TERRAIN_IS_PASSABLE(i, j)) { rows[j].SetRange(i0, i, true); break; } // Check if we reached a jump point if ((!TERRAIN_IS_PASSABLE(i - 1, j - 1) && TERRAIN_IS_PASSABLE(i, j - 1)) || (!TERRAIN_IS_PASSABLE(i - 1, j + 1) && TERRAIN_IS_PASSABLE(i, j + 1))) { rows[j].SetRange(i0, i, false); break; } } } rows[j].Finish(); } #undef TERRAIN_IS_PASSABLE } void reset(const Grid* terrain, pass_class_t passClass) { PROFILE2("JumpPointCache reset"); TIMER(L"JumpPointCache reset"); m_Width = terrain->m_W; m_Height = terrain->m_H; ComputeRows(m_JumpPointsRight, *terrain, passClass, false, false); ComputeRows(m_JumpPointsLeft, *terrain, passClass, false, true); ComputeRows(m_JumpPointsUp, *terrain, passClass, true, false); ComputeRows(m_JumpPointsDown, *terrain, passClass, true, true); } size_t GetMemoryUsage() const { size_t bytes = 0; for (int i = 0; i < m_Width; ++i) { bytes += m_JumpPointsUp[i].GetMemoryUsage(); bytes += m_JumpPointsDown[i].GetMemoryUsage(); } for (int j = 0; j < m_Height; ++j) { bytes += m_JumpPointsRight[j].GetMemoryUsage(); bytes += m_JumpPointsLeft[j].GetMemoryUsage(); } return bytes; } /** * Returns the next jump point (or goal point) to explore, * at (ip, j) where i < ip. * Returns i if there is no such point. */ int GetJumpPointRight(int i, int j, const PathGoal& goal) { int ip; bool obstruction; m_JumpPointsRight[j].Get(i, ip, obstruction); // Adjust ip to be a goal cell, if there is one closer than the jump point; // and then return the new ip if there is a goal, // or the old ip if there is a (non-obstruction) jump point if (goal.NavcellRectContainsGoal(i + 1, j, ip - 1, j, &ip, NULL) || !obstruction) return ip; return i; } int GetJumpPointLeft(int i, int j, const PathGoal& goal) { int mip; // mirrored value, because m_JumpPointsLeft is generated from a mirrored map bool obstruction; m_JumpPointsLeft[j].Get(m_Width - 1 - i, mip, obstruction); int ip = m_Width - 1 - mip; if (goal.NavcellRectContainsGoal(i - 1, j, ip + 1, j, &ip, NULL) || !obstruction) return ip; return i; } int GetJumpPointUp(int i, int j, const PathGoal& goal) { int jp; bool obstruction; m_JumpPointsUp[i].Get(j, jp, obstruction); if (goal.NavcellRectContainsGoal(i, j + 1, i, jp - 1, NULL, &jp) || !obstruction) return jp; return j; } int GetJumpPointDown(int i, int j, const PathGoal& goal) { int mjp; // mirrored value bool obstruction; m_JumpPointsDown[i].Get(m_Height - 1 - j, mjp, obstruction); int jp = m_Height - 1 - mjp; if (goal.NavcellRectContainsGoal(i, j - 1, i, jp + 1, NULL, &jp) || !obstruction) return jp; return j; } }; ////////////////////////////////////////////////////////// LongPathfinder::LongPathfinder() : m_UseJPSCache(false), m_Grid(NULL), m_GridSize(0), m_DebugOverlay(NULL), m_DebugGrid(NULL), m_DebugPath(NULL) { } LongPathfinder::~LongPathfinder() { SAFE_DELETE(m_DebugOverlay); SAFE_DELETE(m_DebugGrid); SAFE_DELETE(m_DebugPath); } #define PASSABLE(i, j) IS_PASSABLE(state.terrain->get(i, j), state.passClass) // 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) const { int di = abs(i - iGoal); int dj = abs(j - jGoal); int diag = std::min(di, dj); return PathCost(di - diag + dj - diag, diag); } // 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) const { // Reject impassable tiles if (!PASSABLE(i, j)) return; PathfindTile& n = state.tiles->get(i, j); if (n.IsClosed()) return; PathCost dg; if (pi == i) dg = PathCost::horizvert(abs(pj - j)); else if (pj == j) dg = PathCost::horizvert(abs(pi - i)); else { ASSERT(abs((int)pi - (int)i) == abs((int)pj - (int)j)); // must be 45 degrees dg = PathCost::diag(abs((int)pi - (int)i)); } PathCost g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor PathCost h = CalculateHeuristic(i, j, state.iGoal, state.jGoal); // If this is a new tile, compute the heuristic distance if (n.IsUnexplored()) { // Remember the best tile we've seen so far, in case we never actually reach the target if (h < state.hBest) { state.hBest = h; state.iBest = i; state.jBest = j; } } else { // If we've already seen this tile, and the new path to this tile does not have a // better cost, then stop now if (g >= n.GetCost()) return; // Otherwise, we have a better path. // If we've already added this tile to the open list: if (n.IsOpen()) { // This is a better path, so replace the old one with the new cost/parent PathCost gprev = n.GetCost(); n.SetCost(g); n.SetPred(pi, pj, i, j); state.open.promote(TileID(i, j), gprev + h, g + h, h); return; } } // Add it to the open list: n.SetStatusOpen(); n.SetCost(g); n.SetPred(pi, pj, i, j); PriorityQueue::Item t = { TileID(i, j), g + h, h }; state.open.push(t); } /* * In the JPS algorithm, after a tile is taken off the open queue, * we don't process every adjacent neighbour (as in standard A*). * Instead we only move in a subset of directions (depending on the * direction from the predecessor); and instead of moving by a single * cell, we move up to the next jump point in that direction. * The AddJumped... functions do this by calling ProcessNeighbour * on the jump point (if any) in a certain direction. * The HasJumped... functions return whether there is any jump point * in that direction. */ // JPS functions scan navcells towards one direction // OnTheWay tests whether we are scanning towards the right direction, to avoid useless scans inline bool OnTheWay(int i, int j, int di, int dj, int iGoal, int jGoal) { if (dj != 0) { // We're not moving towards the goal if ((jGoal - j) * dj < 0) return false; } else if (j != jGoal) return false; if (di != 0) { // We're not moving towards the goal if ((iGoal - i) * di < 0) return false; } else if (i != iGoal) return false; return true; } void LongPathfinder::AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal) const { if (m_UseJPSCache) { int jump; if (di > 0) jump = state.jpc->GetJumpPointRight(i, j, state.goal); else jump = state.jpc->GetJumpPointLeft(i, j, state.goal); if (jump != i) ProcessNeighbour(i, j, jump, j, g, state); } else { ASSERT(di == 1 || di == -1); int ni = i + di; while (true) { if (!PASSABLE(ni, j)) break; if (detectGoal && state.goal.NavcellContainsGoal(ni, j)) { state.open.clear(); ProcessNeighbour(i, j, ni, j, g, state); break; } if ((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) || (!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1))) { ProcessNeighbour(i, j, ni, j, g, state); break; } ni += di; } } } // 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) const { if (m_UseJPSCache) { int jump; if (di > 0) jump = state.jpc->GetJumpPointRight(i, j, state.goal); else jump = state.jpc->GetJumpPointLeft(i, j, state.goal); return jump; } else { ASSERT(di == 1 || di == -1); int ni = i + di; while (true) { if (!PASSABLE(ni, j)) return i; if (detectGoal && state.goal.NavcellContainsGoal(ni, j)) { state.open.clear(); return ni; } if ((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) || (!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1))) return ni; ni += di; } } } void LongPathfinder::AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal) const { if (m_UseJPSCache) { int jump; if (dj > 0) jump = state.jpc->GetJumpPointUp(i, j, state.goal); else jump = state.jpc->GetJumpPointDown(i, j, state.goal); if (jump != j) ProcessNeighbour(i, j, i, jump, g, state); } else { ASSERT(dj == 1 || dj == -1); int nj = j + dj; while (true) { if (!PASSABLE(i, nj)) break; if (detectGoal && state.goal.NavcellContainsGoal(i, nj)) { state.open.clear(); ProcessNeighbour(i, j, i, nj, g, state); break; } if ((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) || (!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj))) { ProcessNeighbour(i, j, i, nj, g, state); break; } nj += dj; } } } // 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) const { if (m_UseJPSCache) { int jump; if (dj > 0) jump = state.jpc->GetJumpPointUp(i, j, state.goal); else jump = state.jpc->GetJumpPointDown(i, j, state.goal); return jump; } else { ASSERT(dj == 1 || dj == -1); int nj = j + dj; while (true) { if (!PASSABLE(i, nj)) return j; if (detectGoal && state.goal.NavcellContainsGoal(i, nj)) { state.open.clear(); return nj; } if ((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) || (!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj))) return nj; nj += dj; } } } /* * 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) const { // ProcessNeighbour(i, j, i + di, j + dj, g, state); // return; ASSERT(di == 1 || di == -1); ASSERT(dj == 1 || dj == -1); int ni = i + di; int nj = j + dj; bool detectGoal = OnTheWay(i, j, di, dj, state.iGoal, state.jGoal); while (true) { // Stop if we hit an obstructed cell if (!PASSABLE(ni, nj)) return; // Stop if moving onto this cell caused us to // touch the corner of an obstructed cell if (!PASSABLE(ni - di, nj) || !PASSABLE(ni, nj - dj)) return; // Process this cell if it's at the goal if (detectGoal && state.goal.NavcellContainsGoal(ni, nj)) { state.open.clear(); ProcessNeighbour(i, j, ni, nj, g, state); return; } int fi = HasJumpedHoriz(ni, nj, di, state, detectGoal && OnTheWay(ni, nj, di, 0, state.iGoal, state.jGoal)); int fj = HasJumpedVert(ni, nj, dj, state, detectGoal && OnTheWay(ni, nj, 0, dj, state.iGoal, state.jGoal)); if (fi != ni || fj != nj) { ProcessNeighbour(i, j, ni, nj, g, state); g += PathCost::diag(abs(ni - i)); if (fi != ni) ProcessNeighbour(ni, nj, fi, nj, g, state); if (fj != nj) ProcessNeighbour(ni, nj, ni, fj, g, state); return; } ni += di; nj += dj; } } -void LongPathfinder::ComputeJPSPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path) const +void LongPathfinder::ComputeJPSPath(const HierarchicalPathfinder& hierPath, 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); PathfinderState state = { 0 }; if (m_UseJPSCache && m_JumpPointCache.find(passClass) == m_JumpPointCache.end()) { state.jpc = new JumpPointCache; state.jpc->reset(m_Grid, passClass); 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); if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass)) { // The JPS pathfinder requires units to be on passable tiles // (otherwise it might crash), so handle the supposedly-invalid // state specially - m_PathfinderHier.FindNearestPassableNavcell(i0, j0, passClass); + hierPath.FindNearestPassableNavcell(i0, j0, passClass); } state.goal = origGoal; // Make the goal reachable. This includes shortening the path if the goal is in a non-passable // region, transforming non-point goals to reachable point goals, etc. - m_PathfinderHier.MakeGoalReachable(i0, j0, state.goal, passClass); + hierPath.MakeGoalReachable(i0, j0, state.goal, passClass); ENSURE(state.goal.type == PathGoal::POINT); // If we're already at the goal tile, then move directly to the exact goal coordinates if (state.goal.NavcellContainsGoal(i0, j0)) { path.m_Waypoints.emplace_back(Waypoint{ state.goal.x, state.goal.z }); return; } Pathfinding::NearestNavcell(state.goal.x, state.goal.z, state.iGoal, state.jGoal, m_GridSize, m_GridSize); ENSURE((state.goal.x / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() == state.iGoal); ENSURE((state.goal.z / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() == state.jGoal); state.passClass = passClass; state.steps = 0; state.tiles = new PathfindTileGrid(m_Grid->m_W, m_Grid->m_H); state.terrain = m_Grid; state.iBest = i0; state.jBest = j0; state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal); PriorityQueue::Item start = { TileID(i0, j0), PathCost() }; state.open.push(start); state.tiles->get(i0, j0).SetStatusOpen(); state.tiles->get(i0, j0).SetPred(i0, j0, i0, j0); state.tiles->get(i0, j0).SetCost(PathCost()); while (true) { ++state.steps; // If we ran out of tiles to examine, give up if (state.open.empty()) break; // Move best tile from open to closed PriorityQueue::Item curr = state.open.pop(); u16 i = curr.id.i(); u16 j = curr.id.j(); state.tiles->get(i, j).SetStatusClosed(); // If we've reached the destination, stop if (state.goal.NavcellContainsGoal(i, j)) { state.iBest = i; state.jBest = j; state.hBest = PathCost(); break; } PathfindTile tile = state.tiles->get(i, j); PathCost g = tile.GetCost(); // Get the direction of the predecessor tile from this tile int dpi = tile.GetPredDI(); int dpj = tile.GetPredDJ(); dpi = (dpi < 0 ? -1 : dpi > 0 ? 1 : 0); dpj = (dpj < 0 ? -1 : dpj > 0 ? 1 : 0); if (dpi != 0 && dpj == 0) { // Moving horizontally from predecessor if (!PASSABLE(i + dpi, j - 1)) { AddJumpedDiag(i, j, -dpi, -1, g, state); AddJumpedVert(i, j, -1, g, state, OnTheWay(i, j, 0, -1, state.iGoal, state.jGoal)); } if (!PASSABLE(i + dpi, j + 1)) { AddJumpedDiag(i, j, -dpi, +1, g, state); AddJumpedVert(i, j, +1, g, state, OnTheWay(i, j, 0, +1, state.iGoal, state.jGoal)); } AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.iGoal, state.jGoal)); } else if (dpi == 0 && dpj != 0) { // Moving vertically from predecessor if (!PASSABLE(i - 1, j + dpj)) { AddJumpedDiag(i, j, -1, -dpj, g, state); AddJumpedHoriz(i, j, -1, g, state,OnTheWay(i, j, -1, 0, state.iGoal, state.jGoal)); } if (!PASSABLE(i + 1, j + dpj)) { AddJumpedDiag(i, j, +1, -dpj, g, state); AddJumpedHoriz(i, j, +1, g, state,OnTheWay(i, j, +1, 0, state.iGoal, state.jGoal)); } AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.iGoal, state.jGoal)); } else if (dpi != 0 && dpj != 0) { // Moving diagonally from predecessor AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.iGoal, state.jGoal)); AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.iGoal, state.jGoal)); AddJumpedDiag(i, j, -dpi, -dpj, g, state); } else { // No predecessor, i.e. the start tile // Start searching in every direction // XXX - check passability? bool passl = PASSABLE(i - 1, j); bool passr = PASSABLE(i + 1, j); bool passd = PASSABLE(i, j - 1); bool passu = PASSABLE(i, j + 1); if (passl && passd) ProcessNeighbour(i, j, i-1, j-1, g, state); if (passr && passd) ProcessNeighbour(i, j, i+1, j-1, g, state); if (passl && passu) ProcessNeighbour(i, j, i-1, j+1, g, state); if (passr && passu) ProcessNeighbour(i, j, i+1, j+1, g, state); if (passl) ProcessNeighbour(i, j, i-1, j, g, state); if (passr) ProcessNeighbour(i, j, i+1, j, g, state); if (passd) ProcessNeighbour(i, j, i, j-1, g, state); if (passu) ProcessNeighbour(i, j, i, j+1, g, state); } } // Reconstruct the path (in reverse) u16 ip = state.iBest, jp = state.jBest; while (ip != i0 || jp != j0) { PathfindTile& n = state.tiles->get(ip, jp); entity_pos_t x, z; Pathfinding::NavcellCenter(ip, jp, x, z); path.m_Waypoints.emplace_back(Waypoint{ x, z }); // Follow the predecessor link ip = n.GetPredI(ip); jp = n.GetPredJ(jp); } // The last waypoint is slightly incorrect (it's not the goal but the center // of the navcell of the goal), so replace it if (!path.m_Waypoints.empty()) path.m_Waypoints.front() = { state.goal.x, state.goal.z }; ImprovePathWaypoints(path, passClass, origGoal.maxdist, x0, z0); // Save this grid for debug display delete m_DebugGrid; 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) const { if (path.m_Waypoints.empty()) return; if (maxDist > fixed::Zero()) { CFixedVector2D start(x0, z0); CFixedVector2D first(path.m_Waypoints.back().x, path.m_Waypoints.back().z); CFixedVector2D offset = first - start; if (offset.CompareLength(maxDist) > 0) { offset.Normalize(maxDist); path.m_Waypoints.emplace_back(Waypoint{ (start + offset).X, (start + offset).Y }); } } if (path.m_Waypoints.size() < 2) return; std::vector& waypoints = path.m_Waypoints; std::vector newWaypoints; CFixedVector2D prev(waypoints.front().x, waypoints.front().z); newWaypoints.push_back(waypoints.front()); for (size_t k = 1; k < waypoints.size() - 1; ++k) { CFixedVector2D ahead(waypoints[k + 1].x, waypoints[k + 1].z); CFixedVector2D curr(waypoints[k].x, waypoints[k].z); if (maxDist > fixed::Zero() && (curr - prev).CompareLength(maxDist) > 0) { // We are too far away from the previous waypoint, so create one in // between and continue with the improvement of the path prev = prev + (curr - prev) / 2; newWaypoints.emplace_back(Waypoint{ prev.X, prev.Y }); } // If we're mostly straight, don't even bother. if ((ahead - curr).Perpendicular().Dot(curr - prev).Absolute() <= fixed::Epsilon() * 100) continue; if (!Pathfinding::CheckLineMovement(prev.X, prev.Y, ahead.X, ahead.Y, passClass, *m_Grid)) { prev = CFixedVector2D(waypoints[k].x, waypoints[k].z); newWaypoints.push_back(waypoints[k]); } } newWaypoints.push_back(waypoints.back()); path.m_Waypoints.swap(newWaypoints); } void LongPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid& grid) const { steps = m_DebugSteps; time = m_DebugTime; if (!m_DebugGrid) return; u16 iGoal, jGoal; Pathfinding::NearestNavcell(m_DebugGoal.x, m_DebugGoal.z, iGoal, jGoal, m_GridSize, m_GridSize); grid = Grid(m_DebugGrid->m_W, m_DebugGrid->m_H); for (u16 j = 0; j < grid.m_H; ++j) { for (u16 i = 0; i < grid.m_W; ++i) { if (i == iGoal && j == jGoal) continue; PathfindTile t = m_DebugGrid->get(i, j); grid.set(i, j, (t.IsOpen() ? 1 : 0) | (t.IsClosed() ? 2 : 0)); } } } void LongPathfinder::SetDebugOverlay(bool enabled) { if (enabled && !m_DebugOverlay) m_DebugOverlay = new LongOverlay(*this); else if (!enabled && m_DebugOverlay) SAFE_DELETE(m_DebugOverlay); } -void LongPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, +void LongPathfinder::ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, + pass_class_t passClass, WaypointPath& path) const +{ + if (!m_Grid) + { + LOGERROR("The pathfinder grid hasn't been setup yet, aborting ComputeJPSPath"); + return; + } + + ComputeJPSPath(hierPath, x0, z0, origGoal, passClass, path); +} +void LongPathfinder::ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, std::vector excludedRegions, WaypointPath& path) { GenerateSpecialMap(passClass, excludedRegions); - ComputePath(x0, z0, origGoal, SPECIAL_PASS_CLASS, path); + ComputeJPSPath(hierPath, x0, z0, origGoal, SPECIAL_PASS_CLASS, path); } inline bool InRegion(u16 i, u16 j, CircularRegion region) { fixed cellX = Pathfinding::NAVCELL_SIZE * i; fixed cellZ = Pathfinding::NAVCELL_SIZE * j; return CFixedVector2D(cellX - region.x, cellZ - region.z).CompareLength(region.r) <= 0; } void LongPathfinder::GenerateSpecialMap(pass_class_t passClass, std::vector excludedRegions) { for (u16 j = 0; j < m_Grid->m_H; ++j) { for (u16 i = 0; i < m_Grid->m_W; ++i) { NavcellData n = m_Grid->get(i, j); if (!IS_PASSABLE(n, passClass)) { n |= SPECIAL_PASS_CLASS; m_Grid->set(i, j, n); continue; } for (CircularRegion& region : excludedRegions) { if (!InRegion(i, j, region)) continue; n |= SPECIAL_PASS_CLASS; break; } m_Grid->set(i, j, n); } } } Index: ps/trunk/source/simulation2/helpers/LongPathfinder.h =================================================================== --- ps/trunk/source/simulation2/helpers/LongPathfinder.h (revision 22277) +++ ps/trunk/source/simulation2/helpers/LongPathfinder.h (revision 22278) @@ -1,402 +1,371 @@ /* Copyright (C) 2017 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * 0 A.D. is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with 0 A.D. If not, see . */ #ifndef INCLUDED_LONGPATHFINDER #define INCLUDED_LONGPATHFINDER #include "Pathfinding.h" -#include "HierarchicalPathfinder.h" -#include "PriorityQueue.h" #include "graphics/Overlay.h" #include "renderer/Scene.h" +#include "renderer/TerrainOverlay.h" +#include "simulation2/helpers/PriorityQueue.h" /** * Represents the 2D coordinates of a tile. * The i/j components are packed into a single u32, since we usually use these * objects for equality comparisons and the VC2010 optimizer doesn't seem to automatically * compare two u16s in a single operation. * TODO: maybe VC2012 will? */ struct TileID { TileID() { } TileID(u16 i, u16 j) : data((i << 16) | j) { } bool operator==(const TileID& b) const { return data == b.data; } /// Returns lexicographic order over (i,j) bool operator<(const TileID& b) const { return data < b.data; } u16 i() const { return data >> 16; } u16 j() const { return data & 0xFFFF; } private: u32 data; }; /** * Tile data for A* computation. * (We store an array of one of these per terrain tile, so it ought to be optimised for size) */ struct PathfindTile { public: enum { STATUS_UNEXPLORED = 0, STATUS_OPEN = 1, STATUS_CLOSED = 2 }; bool IsUnexplored() const { return GetStatus() == STATUS_UNEXPLORED; } bool IsOpen() const { return GetStatus() == STATUS_OPEN; } bool IsClosed() const { return GetStatus() == STATUS_CLOSED; } void SetStatusOpen() { SetStatus(STATUS_OPEN); } void SetStatusClosed() { SetStatus(STATUS_CLOSED); } // Get pi,pj coords of predecessor to this tile on best path, given i,j coords of this tile inline int GetPredI(int i) const { return i + GetPredDI(); } inline int GetPredJ(int j) const { return j + GetPredDJ(); } inline PathCost GetCost() const { return g; } inline void SetCost(PathCost cost) { g = cost; } private: PathCost g; // cost to reach this tile u32 data; // 2-bit status; 15-bit PredI; 15-bit PredJ; packed for storage efficiency public: inline u8 GetStatus() const { return data & 3; } inline void SetStatus(u8 s) { ASSERT(s < 4); data &= ~3; data |= (s & 3); } int GetPredDI() const { return (i32)data >> 17; } int GetPredDJ() const { return ((i32)data << 15) >> 17; } // Set the pi,pj coords of predecessor, given i,j coords of this tile inline void SetPred(int pi, int pj, int i, int j) { int di = pi - i; int dj = pj - j; ASSERT(-16384 <= di && di < 16384); ASSERT(-16384 <= dj && dj < 16384); data &= 3; data |= (((u32)di & 0x7FFF) << 17) | (((u32)dj & 0x7FFF) << 2); } }; struct CircularRegion { CircularRegion(entity_pos_t x, entity_pos_t z, entity_pos_t r) : x(x), z(z), r(r) {} entity_pos_t x, z, r; }; typedef PriorityQueueHeap PriorityQueue; typedef SparseGrid PathfindTileGrid; class JumpPointCache; struct PathfinderState { u32 steps; // number of algorithm iterations PathGoal goal; u16 iGoal, jGoal; // goal tile pass_class_t passClass; PriorityQueue open; // (there's no explicit closed list; it's encoded in PathfindTile) PathfindTileGrid* tiles; Grid* terrain; PathCost hBest; // heuristic of closest discovered tile to goal u16 iBest, jBest; // closest tile JumpPointCache* jpc; }; class LongOverlay; +class HierarchicalPathfinder; + class LongPathfinder { public: LongPathfinder(); ~LongPathfinder(); void SetDebugOverlay(bool enabled); - void SetHierDebugOverlay(bool enabled, const CSimContext *simContext) - { - m_PathfinderHier.SetDebugOverlay(enabled, simContext); - } - - void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) + void SetDebugPath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) { if (!m_DebugOverlay) return; SAFE_DELETE(m_DebugGrid); delete m_DebugPath; m_DebugPath = new WaypointPath(); - ComputePath(x0, z0, goal, passClass, *m_DebugPath); + ComputePath(hierPath, x0, z0, goal, passClass, *m_DebugPath); m_DebugPassClass = passClass; } - void Reload(Grid* passabilityGrid, - const std::map& nonPathfindingPassClassMasks, - const std::map& pathfindingPassClassMasks) + void Reload(Grid* passabilityGrid) { m_Grid = passabilityGrid; ASSERT(passabilityGrid->m_H == passabilityGrid->m_W); m_GridSize = passabilityGrid->m_W; m_JumpPointCache.clear(); - - m_PathfinderHier.Recompute(passabilityGrid, nonPathfindingPassClassMasks, pathfindingPassClassMasks); } - void Update(Grid* passabilityGrid, const Grid& dirtinessGrid) + void Update(Grid* passabilityGrid) { m_Grid = passabilityGrid; ASSERT(passabilityGrid->m_H == passabilityGrid->m_W); ASSERT(m_GridSize == passabilityGrid->m_H); m_JumpPointCache.clear(); - - m_PathfinderHier.Update(passabilityGrid, dirtinessGrid); - } - - void HierarchicalRenderSubmit(SceneCollector& collector) - { - for (size_t i = 0; i < m_PathfinderHier.m_DebugOverlayLines.size(); ++i) - collector.Submit(&m_PathfinderHier.m_DebugOverlayLines[i]); } /** * 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& origGoal, - pass_class_t passClass, WaypointPath& path) const - { - if (!m_Grid) - { - LOGERROR("The pathfinder grid hasn't been setup yet, aborting ComputePath"); - return; - } - - ComputeJPSPath(x0, z0, origGoal, passClass, path); - } + void ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, + pass_class_t passClass, WaypointPath& path) const; /** * Compute a tile-based path from the given point to the goal, excluding the regions * specified in excludedRegions (which are treated as impassable) 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& origGoal, + void ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, std::vector excludedRegions, WaypointPath& path); - Grid GetConnectivityGrid(pass_class_t passClass) - { - return m_PathfinderHier.GetConnectivityGrid(passClass); - } - void GetDebugData(u32& steps, double& time, Grid& grid) const { GetDebugDataJPS(steps, time, grid); } Grid* m_Grid; u16 m_GridSize; // Debugging - output from last pathfind operation. // mutable as making these const would require a lot of boilerplate code // and they do not change the behavioural const-ness of the pathfinder. 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; private: 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) 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) const; + void ComputeJPSPath(const HierarchicalPathfinder& hierPath, 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 /** * Given a path with an arbitrary collection of waypoints, updates the * waypoints to be nicer. Calls "Testline" between waypoints * so that bended paths can become straight if there's nothing in between * (this happens because A* is 8-direction, and the map isn't actually a grid). * 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) const; /** * Generate a passability map, stored in the 16th bit of navcells, based on passClass, * but with a set of impassable circular regions. */ void GenerateSpecialMap(pass_class_t passClass, std::vector excludedRegions); bool m_UseJPSCache; // Mutable may be used here as caching does not change the external const-ness of the Long Range pathfinder. // This is thread-safe as it is order independent (no change in the output of the function for a given set of params). // Obviously, this means that the cache should actually be a cache and not return different results // from what would happen if things hadn't been cached. mutable std::map > m_JumpPointCache; - - HierarchicalPathfinder m_PathfinderHier; }; /** * Terrain overlay for pathfinder debugging. * Renders a representation of the most recent pathfinding operation. */ class LongOverlay : public TerrainTextureOverlay { public: LongPathfinder& m_Pathfinder; LongOverlay(LongPathfinder& pathfinder) : TerrainTextureOverlay(Pathfinding::NAVCELLS_PER_TILE), m_Pathfinder(pathfinder) { } virtual void BuildTextureRGBA(u8* data, size_t w, size_t h) { // Grab the debug data for the most recently generated path u32 steps; double time; Grid debugGrid; m_Pathfinder.GetDebugData(steps, time, debugGrid); // Render navcell passability u8* p = data; for (size_t j = 0; j < h; ++j) { for (size_t i = 0; i < w; ++i) { SColor4ub color(0, 0, 0, 0); if (!IS_PASSABLE(m_Pathfinder.m_Grid->get((int)i, (int)j), m_Pathfinder.m_DebugPassClass)) color = SColor4ub(255, 0, 0, 127); if (debugGrid.m_W && debugGrid.m_H) { u8 n = debugGrid.get((int)i, (int)j); if (n == 1) color = SColor4ub(255, 255, 0, 127); else if (n == 2) color = SColor4ub(0, 255, 0, 127); if (m_Pathfinder.m_DebugGoal.NavcellContainsGoal(i, j)) color = SColor4ub(0, 0, 255, 127); } *p++ = color.R; *p++ = color.G; *p++ = color.B; *p++ = color.A; } } // Render the most recently generated path if (m_Pathfinder.m_DebugPath && !m_Pathfinder.m_DebugPath->m_Waypoints.empty()) { std::vector& waypoints = m_Pathfinder.m_DebugPath->m_Waypoints; u16 ip = 0, jp = 0; for (size_t k = 0; k < waypoints.size(); ++k) { u16 i, j; Pathfinding::NearestNavcell(waypoints[k].x, waypoints[k].z, i, j, m_Pathfinder.m_GridSize, m_Pathfinder.m_GridSize); if (k == 0) { ip = i; jp = j; } else { bool firstCell = true; do { if (data[(jp*w + ip)*4+3] == 0) { data[(jp*w + ip)*4+0] = 0xFF; data[(jp*w + ip)*4+1] = 0xFF; data[(jp*w + ip)*4+2] = 0xFF; data[(jp*w + ip)*4+3] = firstCell ? 0xA0 : 0x60; } ip = ip < i ? ip+1 : ip > i ? ip-1 : ip; jp = jp < j ? jp+1 : jp > j ? jp-1 : jp; firstCell = false; } while (ip != i || jp != j); } } } } }; #endif // INCLUDED_LONGPATHFINDER