Index: ps/trunk/source/simulation2/components/CCmpPathfinder.cpp =================================================================== --- ps/trunk/source/simulation2/components/CCmpPathfinder.cpp +++ ps/trunk/source/simulation2/components/CCmpPathfinder.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2018 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -35,6 +35,7 @@ #include "simulation2/components/ICmpTerrain.h" #include "simulation2/components/ICmpWaterManager.h" #include "simulation2/helpers/Rasterize.h" +#include "simulation2/helpers/VertexPathfinder.h" #include "simulation2/serialization/SerializeTemplates.h" REGISTER_COMPONENT_TYPE(Pathfinder) @@ -49,11 +50,12 @@ m_NextAsyncTicket = 1; - m_DebugOverlay = false; m_AtlasOverlay = NULL; m_SameTurnMovesCount = 0; + m_VertexPathfinder = std::unique_ptr(new VertexPathfinder(m_MapSize, m_TerrainOnlyGrid)); + // Register Relax NG validator CXeromyces::AddValidator(g_VFS, "pathfinder", "simulation/data/pathfinder.rng"); @@ -93,6 +95,8 @@ } } +CCmpPathfinder::~CCmpPathfinder() {}; + void CCmpPathfinder::Deinit() { SetDebugOverlay(false); // cleans up memory @@ -183,12 +187,31 @@ void CCmpPathfinder::RenderSubmit(SceneCollector& collector) { - for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i) - collector.Submit(&m_DebugOverlayShortPathLines[i]); - + m_VertexPathfinder->RenderSubmit(collector); m_LongPathfinder.HierarchicalRenderSubmit(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); +} + +void CCmpPathfinder::SetDebugOverlay(bool enabled) +{ + m_VertexPathfinder->SetDebugOverlay(enabled); + m_LongPathfinder.SetDebugOverlay(enabled); +} + +void CCmpPathfinder::SetHierDebugOverlay(bool enabled) +{ + m_LongPathfinder.SetHierDebugOverlay(enabled, &GetSimContext()); +} + +void CCmpPathfinder::GetDebugData(u32& steps, double& time, Grid& grid) const +{ + m_LongPathfinder.GetDebugData(steps, time, grid); +} + void CCmpPathfinder::SetAtlasOverlay(bool enable, pass_class_t passClass) { if (enable) @@ -672,8 +695,6 @@ ////////////////////////////////////////////////////////// -// Async path requests: - u32 CCmpPathfinder::ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) { AsyncLongPathRequest req = { m_NextAsyncTicket++, x0, z0, goal, passClass, notify }; @@ -688,6 +709,13 @@ 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"); @@ -726,9 +754,7 @@ for (size_t i = 0; i < shortRequests.size(); ++i) { const AsyncShortPathRequest& req = shortRequests[i]; - WaypointPath path; - ControlGroupMovementObstructionFilter filter(req.avoidMovingUnits, req.group); - ComputeShortPath(filter, req.x0, req.z0, req.clearance, req.range, req.goal, req.passClass, path); + WaypointPath path = m_VertexPathfinder->ComputeShortPath(req, CmpPtr(GetSystemEntity())); CMessagePathResult msg(req.ticket, path); GetSimContext().GetComponentManager().PostMessage(req.notify, msg); } Index: ps/trunk/source/simulation2/components/CCmpPathfinder_Common.h =================================================================== --- ps/trunk/source/simulation2/components/CCmpPathfinder_Common.h +++ ps/trunk/source/simulation2/components/CCmpPathfinder_Common.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2017 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -35,9 +35,12 @@ #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 VertexPathfinder; + class SceneCollector; class AtlasOverlay; @@ -47,78 +50,10 @@ #define PATHFIND_DEBUG 1 #endif - -struct AsyncLongPathRequest -{ - u32 ticket; - entity_pos_t x0; - entity_pos_t z0; - PathGoal goal; - pass_class_t passClass; - entity_id_t notify; -}; - -struct AsyncShortPathRequest -{ - u32 ticket; - entity_pos_t x0; - entity_pos_t z0; - entity_pos_t clearance; - entity_pos_t range; - PathGoal goal; - pass_class_t passClass; - bool avoidMovingUnits; - entity_id_t group; - entity_id_t notify; -}; - -// A vertex around the corners of an obstruction -// (paths will be sequences of these vertexes) -struct Vertex -{ - enum - { - UNEXPLORED, - OPEN, - CLOSED, - }; - - CFixedVector2D p; - fixed g, h; - u16 pred; - u8 status; - u8 quadInward : 4; // the quadrant which is inside the shape (or NONE) - u8 quadOutward : 4; // the quadrants of the next point on the path which this vertex must be in, given 'pred' -}; - -// Obstruction edges (paths will not cross any of these). -// Defines the two points of the edge. -struct Edge -{ - CFixedVector2D p0, p1; -}; - -// Axis-aligned obstruction squares (paths will not cross any of these). -// Defines the opposing corners of an axis-aligned square -// (from which four individual edges can be trivially computed), requiring p0 <= p1 -struct Square -{ - CFixedVector2D p0, p1; -}; - -// Axis-aligned obstruction edges. -// p0 defines one end; c1 is either the X or Y coordinate of the other end, -// depending on the context in which this is used. -struct EdgeAA -{ - CFixedVector2D p0; - fixed c1; -}; - /** * Implementation of ICmpPathfinder */ -class CCmpPathfinder : public ICmpPathfinder +class CCmpPathfinder final : public ICmpPathfinder { public: static void ClassInit(CComponentManager& componentManager) @@ -131,6 +66,8 @@ componentManager.SubscribeToMessageType(MT_TurnStart); } + ~CCmpPathfinder(); + DEFAULT_COMPONENT_ALLOCATOR(Pathfinder) // Template state: @@ -158,6 +95,7 @@ GridUpdateInformation m_AIPathfinderDirtinessInformation; bool m_TerrainDirty; + std::unique_ptr m_VertexPathfinder; // Interface to the long-range pathfinder. LongPathfinder m_LongPathfinder; @@ -165,23 +103,6 @@ u16 m_MaxSameTurnMoves; // max number of moves that can be created and processed in the same turn - // memory optimizations: those vectors are created once, reused for all calculations; - std::vector edgesUnaligned; - std::vector edgesLeft; - std::vector edgesRight; - std::vector edgesBottom; - std::vector edgesTop; - - // List of obstruction vertexes (plus start/end points); we'll try to find paths through - // the graph defined by these vertexes - std::vector vertexes; - // List of collision edges - paths must never cross these. - // (Edges are one-sided so intersections are fine in one direction, but not the other direction.) - std::vector edges; - std::vector edgeSquares; // axis-aligned squares; equivalent to 4 edges - - bool m_DebugOverlay; - std::vector m_DebugOverlayShortPathLines; AtlasOverlay* m_AtlasOverlay; static std::string GetSchema() @@ -252,30 +173,17 @@ virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify); - virtual void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret); + virtual 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) - { - m_LongPathfinder.SetDebugPath(x0, z0, goal, passClass); - } + virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass); - virtual void SetDebugOverlay(bool enabled) - { - m_DebugOverlay = enabled; - m_LongPathfinder.SetDebugOverlay(enabled); - } + virtual void SetDebugOverlay(bool enabled); - virtual void SetHierDebugOverlay(bool enabled) - { - m_LongPathfinder.SetHierDebugOverlay(enabled, &GetSimContext()); - } + virtual void SetHierDebugOverlay(bool enabled); - virtual void GetDebugData(u32& steps, double& time, Grid& grid) const - { - m_LongPathfinder.GetDebugData(steps, time, grid); - } + virtual void GetDebugData(u32& steps, double& time, Grid& grid) const; virtual void SetAtlasOverlay(bool enable, pass_class_t passClass = 0); Index: ps/trunk/source/simulation2/components/CCmpPathfinder_Vertex.cpp =================================================================== --- ps/trunk/source/simulation2/components/CCmpPathfinder_Vertex.cpp +++ ps/trunk/source/simulation2/components/CCmpPathfinder_Vertex.cpp @@ -1,931 +0,0 @@ -/* 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 . - */ - -/** - * @file - * Vertex-based algorithm for CCmpPathfinder. - * Computes paths around the corners of rectangular obstructions. - * - * Useful search term for this algorithm: "points of visibility". - * - * Since we sometimes want to use this for avoiding moving units, there is no - * pre-computation - the whole visibility graph is effectively regenerated for - * each path, and it does A* over that graph. - * - * This scales very poorly in the number of obstructions, so it should be used - * with a limited range and not exceedingly frequently. - */ - -#include "precompiled.h" - -#include "CCmpPathfinder_Common.h" - -#include "lib/timer.h" -#include "ps/Profile.h" -#include "simulation2/components/ICmpObstructionManager.h" -#include "simulation2/helpers/PriorityQueue.h" -#include "simulation2/helpers/Render.h" - -/* Quadrant optimisation: - * (loosely based on GPG2 "Optimizing Points-of-Visibility Pathfinding") - * - * Consider the vertex ("@") at a corner of an axis-aligned rectangle ("#"): - * - * TL : TR - * : - * ####@ - - - - * ##### - * ##### - * BL ## BR - * - * The area around the vertex is split into TopLeft, BottomRight etc quadrants. - * - * If the shortest path reaches this vertex, it cannot continue to a vertex in - * the BL quadrant (it would be blocked by the shape). - * Since the shortest path is wrapped tightly around the edges of obstacles, - * if the path approached this vertex from the TL quadrant, - * it cannot continue to the TL or TR quadrants (the path could be shorter if it - * skipped this vertex). - * Therefore it must continue to a vertex in the BR quadrant (so this vertex is in - * *that* vertex's TL quadrant). - * - * That lets us significantly reduce the search space by quickly discarding vertexes - * from the wrong quadrants. - * - * (This causes badness if the path starts from inside the shape, so we add some hacks - * for that case.) - * - * (For non-axis-aligned rectangles it's harder to do this computation, so we'll - * not bother doing any discarding for those.) - */ -static const u8 QUADRANT_NONE = 0; -static const u8 QUADRANT_BL = 1; -static const u8 QUADRANT_TR = 2; -static const u8 QUADRANT_TL = 4; -static const u8 QUADRANT_BR = 8; -static const u8 QUADRANT_BLTR = QUADRANT_BL|QUADRANT_TR; -static const u8 QUADRANT_TLBR = QUADRANT_TL|QUADRANT_BR; -static const u8 QUADRANT_ALL = QUADRANT_BLTR|QUADRANT_TLBR; - -// When computing vertexes to insert into the search graph, -// add a small delta so that the vertexes of an edge don't get interpreted -// as crossing the edge (given minor numerical inaccuracies) -static const entity_pos_t EDGE_EXPAND_DELTA = entity_pos_t::FromInt(1)/16; - -/** - * Check whether a ray from 'a' to 'b' crosses any of the edges. - * (Edges are one-sided so it's only considered a cross if going from front to back.) - */ -inline static bool CheckVisibility(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) -{ - CFixedVector2D abn = (b - a).Perpendicular(); - - // Edges of general non-axis-aligned shapes - for (size_t i = 0; i < edges.size(); ++i) - { - CFixedVector2D p0 = edges[i].p0; - CFixedVector2D p1 = edges[i].p1; - - CFixedVector2D d = (p1 - p0).Perpendicular(); - - // If 'a' is behind the edge, we can't cross - fixed q = (a - p0).Dot(d); - if (q < fixed::Zero()) - continue; - - // If 'b' is in front of the edge, we can't cross - fixed r = (b - p0).Dot(d); - if (r > fixed::Zero()) - continue; - - // The ray is crossing the infinitely-extended edge from in front to behind. - // Check the finite edge is crossing the infinitely-extended ray too. - // (Given the previous tests, it can only be crossing in one direction.) - fixed s = (p0 - a).Dot(abn); - if (s > fixed::Zero()) - continue; - - fixed t = (p1 - a).Dot(abn); - if (t < fixed::Zero()) - continue; - - return false; - } - - return true; -} - -// Handle the axis-aligned shape edges separately (for performance): -// (These are specialised versions of the general unaligned edge code. -// They assume the caller has already excluded edges for which 'a' is -// on the wrong side.) - -inline static bool CheckVisibilityLeft(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) -{ - if (a.X >= b.X) - return true; - - CFixedVector2D abn = (b - a).Perpendicular(); - - for (size_t i = 0; i < edges.size(); ++i) - { - if (b.X < edges[i].p0.X) - continue; - - CFixedVector2D p0 (edges[i].p0.X, edges[i].c1); - fixed s = (p0 - a).Dot(abn); - if (s > fixed::Zero()) - continue; - - CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y); - fixed t = (p1 - a).Dot(abn); - if (t < fixed::Zero()) - continue; - - return false; - } - - return true; -} - -inline static bool CheckVisibilityRight(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) -{ - if (a.X <= b.X) - return true; - - CFixedVector2D abn = (b - a).Perpendicular(); - - for (size_t i = 0; i < edges.size(); ++i) - { - if (b.X > edges[i].p0.X) - continue; - - CFixedVector2D p0 (edges[i].p0.X, edges[i].c1); - fixed s = (p0 - a).Dot(abn); - if (s > fixed::Zero()) - continue; - - CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y); - fixed t = (p1 - a).Dot(abn); - if (t < fixed::Zero()) - continue; - - return false; - } - - return true; -} - -inline static bool CheckVisibilityBottom(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) -{ - if (a.Y >= b.Y) - return true; - - CFixedVector2D abn = (b - a).Perpendicular(); - - for (size_t i = 0; i < edges.size(); ++i) - { - if (b.Y < edges[i].p0.Y) - continue; - - CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y); - fixed s = (p0 - a).Dot(abn); - if (s > fixed::Zero()) - continue; - - CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y); - fixed t = (p1 - a).Dot(abn); - if (t < fixed::Zero()) - continue; - - return false; - } - - return true; -} - -inline static bool CheckVisibilityTop(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) -{ - if (a.Y <= b.Y) - return true; - - CFixedVector2D abn = (b - a).Perpendicular(); - - for (size_t i = 0; i < edges.size(); ++i) - { - if (b.Y > edges[i].p0.Y) - continue; - - CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y); - fixed s = (p0 - a).Dot(abn); - if (s > fixed::Zero()) - continue; - - CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y); - fixed t = (p1 - a).Dot(abn); - if (t < fixed::Zero()) - continue; - - return false; - } - - return true; -} - -typedef PriorityQueueHeap VertexPriorityQueue; - -/** - * Add edges and vertexes to represent the boundaries between passable and impassable - * navcells (for impassable terrain). - * Navcells i0 <= i <= i1, j0 <= j <= j1 will be considered. - */ -static void AddTerrainEdges(std::vector& edges, std::vector& vertexes, - int i0, int j0, int i1, int j1, - pass_class_t passClass, const Grid& grid) -{ - PROFILE("AddTerrainEdges"); - - // Clamp the coordinates so we won't attempt to sample outside of the grid. - // (This assumes the outermost ring of navcells (which are always impassable) - // won't have a boundary with any passable navcells. TODO: is that definitely - // safe enough?) - - i0 = clamp(i0, 1, grid.m_W-2); - j0 = clamp(j0, 1, grid.m_H-2); - i1 = clamp(i1, 1, grid.m_W-2); - j1 = clamp(j1, 1, grid.m_H-2); - - for (int j = j0; j <= j1; ++j) - { - for (int i = i0; i <= i1; ++i) - { - if (IS_PASSABLE(grid.get(i, j), passClass)) - continue; - - if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i+1, j+1), passClass)) - { - Vertex vert; - vert.status = Vertex::UNEXPLORED; - vert.quadOutward = QUADRANT_ALL; - vert.quadInward = QUADRANT_BL; - vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); - vertexes.push_back(vert); - } - - if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i-1, j+1), passClass)) - { - Vertex vert; - vert.status = Vertex::UNEXPLORED; - vert.quadOutward = QUADRANT_ALL; - vert.quadInward = QUADRANT_BR; - vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); - vertexes.push_back(vert); - } - - if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i+1, j-1), passClass)) - { - Vertex vert; - vert.status = Vertex::UNEXPLORED; - vert.quadOutward = QUADRANT_ALL; - vert.quadInward = QUADRANT_TL; - vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); - vertexes.push_back(vert); - } - - if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i-1, j-1), passClass)) - { - Vertex vert; - vert.status = Vertex::UNEXPLORED; - vert.quadOutward = QUADRANT_ALL; - vert.quadInward = QUADRANT_TR; - vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); - vertexes.push_back(vert); - } - } - } - - // XXX rewrite this stuff - std::vector segmentsR; - std::vector segmentsL; - for (int j = j0; j < j1; ++j) - { - segmentsR.clear(); - segmentsL.clear(); - for (int i = i0; i <= i1; ++i) - { - bool a = IS_PASSABLE(grid.get(i, j+1), passClass); - bool b = IS_PASSABLE(grid.get(i, j), passClass); - if (a && !b) - segmentsL.push_back(i); - if (b && !a) - segmentsR.push_back(i); - } - - if (!segmentsR.empty()) - { - segmentsR.push_back(0); // sentinel value to simplify the loop - u16 ia = segmentsR[0]; - u16 ib = ia + 1; - for (size_t n = 1; n < segmentsR.size(); ++n) - { - if (segmentsR[n] == ib) - ++ib; - else - { - CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); - CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); - edges.emplace_back(Edge{ v0, v1 }); - - ia = segmentsR[n]; - ib = ia + 1; - } - } - } - - if (!segmentsL.empty()) - { - segmentsL.push_back(0); // sentinel value to simplify the loop - u16 ia = segmentsL[0]; - u16 ib = ia + 1; - for (size_t n = 1; n < segmentsL.size(); ++n) - { - if (segmentsL[n] == ib) - ++ib; - else - { - CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); - CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); - edges.emplace_back(Edge{ v0, v1 }); - - ia = segmentsL[n]; - ib = ia + 1; - } - } - } - } - std::vector segmentsU; - std::vector segmentsD; - for (int i = i0; i < i1; ++i) - { - segmentsU.clear(); - segmentsD.clear(); - for (int j = j0; j <= j1; ++j) - { - bool a = IS_PASSABLE(grid.get(i+1, j), passClass); - bool b = IS_PASSABLE(grid.get(i, j), passClass); - if (a && !b) - segmentsU.push_back(j); - if (b && !a) - segmentsD.push_back(j); - } - - if (!segmentsU.empty()) - { - segmentsU.push_back(0); // sentinel value to simplify the loop - u16 ja = segmentsU[0]; - u16 jb = ja + 1; - for (size_t n = 1; n < segmentsU.size(); ++n) - { - if (segmentsU[n] == jb) - ++jb; - else - { - CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE); - CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE); - edges.emplace_back(Edge{ v0, v1 }); - - ja = segmentsU[n]; - jb = ja + 1; - } - } - } - - if (!segmentsD.empty()) - { - segmentsD.push_back(0); // sentinel value to simplify the loop - u16 ja = segmentsD[0]; - u16 jb = ja + 1; - for (size_t n = 1; n < segmentsD.size(); ++n) - { - if (segmentsD[n] == jb) - ++jb; - else - { - CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE); - CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE); - edges.emplace_back(Edge{ v0, v1 }); - - ja = segmentsD[n]; - jb = ja + 1; - } - } - } - } -} - -static void SplitAAEdges(const CFixedVector2D& a, - const std::vector& edges, - const std::vector& squares, - std::vector& edgesUnaligned, - std::vector& edgesLeft, std::vector& edgesRight, - std::vector& edgesBottom, std::vector& edgesTop) -{ - - for (const Square& square : squares) - { - if (a.X <= square.p0.X) - edgesLeft.emplace_back(EdgeAA{ square.p0, square.p1.Y }); - if (a.X >= square.p1.X) - edgesRight.emplace_back(EdgeAA{ square.p1, square.p0.Y }); - if (a.Y <= square.p0.Y) - edgesBottom.emplace_back(EdgeAA{ square.p0, square.p1.X }); - if (a.Y >= square.p1.Y) - edgesTop.emplace_back(EdgeAA{ square.p1, square.p0.X }); - } - - for (const Edge& edge : edges) - { - if (edge.p0.X == edge.p1.X) - { - if (edge.p1.Y < edge.p0.Y) - { - if (!(a.X <= edge.p0.X)) - continue; - edgesLeft.emplace_back(EdgeAA{ edge.p1, edge.p0.Y }); - } - else - { - if (!(a.X >= edge.p0.X)) - continue; - edgesRight.emplace_back(EdgeAA{ edge.p1, edge.p0.Y }); - } - } - else if (edge.p0.Y == edge.p1.Y) - { - if (edge.p0.X < edge.p1.X) - { - if (!(a.Y <= edge.p0.Y)) - continue; - edgesBottom.emplace_back(EdgeAA{ edge.p0, edge.p1.X }); - } - else - { - if (!(a.Y >= edge.p0.Y)) - continue; - edgesTop.emplace_back(EdgeAA{ edge.p0, edge.p1.X }); - } - } - else - edgesUnaligned.push_back(edge); - } -} - -/** - * Functor for sorting edge-squares by approximate proximity to a fixed point. - */ -struct SquareSort -{ - CFixedVector2D src; - SquareSort(CFixedVector2D src) : src(src) { } - bool operator()(const Square& a, const Square& b) const - { - if ((a.p0 - src).CompareLength(b.p0 - src) < 0) - return true; - return false; - } -}; - -void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, - entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, - entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& path) -{ - PROFILE("ComputeShortPath"); - m_DebugOverlayShortPathLines.clear(); - - if (m_DebugOverlay) - { - // Render the goal shape - m_DebugOverlayShortPathLines.push_back(SOverlayLine()); - m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1); - switch (goal.type) - { - case PathGoal::POINT: - { - SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true); - break; - } - case PathGoal::CIRCLE: - case PathGoal::INVERTED_CIRCLE: - { - SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true); - break; - } - case PathGoal::SQUARE: - case PathGoal::INVERTED_SQUARE: - { - float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat()); - SimRender::ConstructSquareOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true); - break; - } - } - } - - // List of collision edges - paths must never cross these. - // (Edges are one-sided so intersections are fine in one direction, but not the other direction.) - edges.clear(); - edgeSquares.clear(); // axis-aligned squares; equivalent to 4 edges - - // Create impassable edges at the max-range boundary, so we can't escape the region - // where we're meant to be searching - fixed rangeXMin = x0 - range; - fixed rangeXMax = x0 + range; - fixed rangeZMin = z0 - range; - fixed rangeZMax = z0 + range; - - // we don't actually add the "search space" edges as edges, since we may want to cross them - // in some cases (such as if we need to go around an obstruction that's partly out of the search range) - - // List of obstruction vertexes (plus start/end points); we'll try to find paths through - // the graph defined by these vertexes - vertexes.clear(); - - // Add the start point to the graph - CFixedVector2D posStart(x0, z0); - fixed hStart = (posStart - goal.NearestPointOnGoal(posStart)).Length(); - Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL }; - vertexes.push_back(start); - const size_t START_VERTEX_ID = 0; - - // Add the goal vertex to the graph. - // Since the goal isn't always a point, this a special magic virtual vertex which moves around - whenever - // we look at it from another vertex, it is moved to be the closest point on the goal shape to that vertex. - Vertex end = { CFixedVector2D(goal.x, goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL }; - vertexes.push_back(end); - const size_t GOAL_VERTEX_ID = 1; - - // Find all the obstruction squares that might affect us - CmpPtr cmpObstructionManager(GetSystemEntity()); - std::vector squares; - size_t staticShapesNb = 0; - cmpObstructionManager->GetStaticObstructionsInRange(filter, rangeXMin - clearance, rangeZMin - clearance, rangeXMax + clearance, rangeZMax + clearance, squares); - staticShapesNb = squares.size(); - cmpObstructionManager->GetUnitObstructionsInRange(filter, rangeXMin - clearance, rangeZMin - clearance, rangeXMax + clearance, rangeZMax + clearance, squares); - - // Change array capacities to reduce reallocations - vertexes.reserve(vertexes.size() + squares.size()*4); - edgeSquares.reserve(edgeSquares.size() + squares.size()); // (assume most squares are AA) - - entity_pos_t pathfindClearance = clearance; - - // Convert each obstruction square into collision edges and search graph vertexes - for (size_t i = 0; i < squares.size(); ++i) - { - CFixedVector2D center(squares[i].x, squares[i].z); - CFixedVector2D u = squares[i].u; - CFixedVector2D v = squares[i].v; - - if (i >= staticShapesNb) - pathfindClearance = clearance - entity_pos_t::FromInt(1)/2; - - // Expand the vertexes by the moving unit's collision radius, to find the - // closest we can get to it - - CFixedVector2D hd0(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA, squares[i].hh + pathfindClearance + EDGE_EXPAND_DELTA); - CFixedVector2D hd1(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA, -(squares[i].hh + pathfindClearance + EDGE_EXPAND_DELTA)); - - // Check whether this is an axis-aligned square - bool aa = (u.X == fixed::FromInt(1) && u.Y == fixed::Zero() && v.X == fixed::Zero() && v.Y == fixed::FromInt(1)); - - Vertex vert; - vert.status = Vertex::UNEXPLORED; - vert.quadInward = QUADRANT_NONE; - vert.quadOutward = QUADRANT_ALL; - vert.p.X = center.X - hd0.Dot(u); vert.p.Y = center.Y + hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_BR; vertexes.push_back(vert); - if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; - if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; - if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; - if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; - vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_TR; vertexes.push_back(vert); - if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; - if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; - if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; - if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; - vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_TL; vertexes.push_back(vert); - if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; - if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; - if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; - if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; - vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; vertexes.push_back(vert); - if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; - if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; - if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; - if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; - - // Add the edges: - - CFixedVector2D h0(squares[i].hw + pathfindClearance, squares[i].hh + pathfindClearance); - CFixedVector2D h1(squares[i].hw + pathfindClearance, -(squares[i].hh + pathfindClearance)); - - CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v)); - CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v)); - CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v)); - CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v)); - if (aa) - edgeSquares.emplace_back(Square{ ev1, ev3 }); - else - { - edges.emplace_back(Edge{ ev0, ev1 }); - edges.emplace_back(Edge{ ev1, ev2 }); - edges.emplace_back(Edge{ ev2, ev3 }); - edges.emplace_back(Edge{ ev3, ev0 }); - } - - // TODO: should clip out vertexes and edges that are outside the range, - // to reduce the search space - } - - // Add terrain obstructions - { - u16 i0, j0, i1, j1; - Pathfinding::NearestNavcell(rangeXMin, rangeZMin, i0, j0, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); - Pathfinding::NearestNavcell(rangeXMax, rangeZMax, i1, j1, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); - AddTerrainEdges(edges, vertexes, i0, j0, i1, j1, passClass, *m_TerrainOnlyGrid); - } - - // Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable) - for (size_t i = 0; i < edgeSquares.size(); ++i) - { - // If the start point is inside the square, ignore it - if (start.p.X >= edgeSquares[i].p0.X && - start.p.Y >= edgeSquares[i].p0.Y && - start.p.X <= edgeSquares[i].p1.X && - start.p.Y <= edgeSquares[i].p1.Y) - continue; - - // Remove every non-start/goal vertex that is inside an edgeSquare; - // since remove() would be inefficient, just mark it as closed instead. - for (size_t j = 2; j < vertexes.size(); ++j) - if (vertexes[j].p.X >= edgeSquares[i].p0.X && - vertexes[j].p.Y >= edgeSquares[i].p0.Y && - vertexes[j].p.X <= edgeSquares[i].p1.X && - vertexes[j].p.Y <= edgeSquares[i].p1.Y) - vertexes[j].status = Vertex::CLOSED; - } - - ENSURE(vertexes.size() < 65536); // we store array indexes as u16 - - // Render the debug overlay - if (m_DebugOverlay) - { -#define PUSH_POINT(p) STMT(xz.push_back(p.X.ToFloat()); xz.push_back(p.Y.ToFloat())) - // Render the vertexes as little Pac-Man shapes to indicate quadrant direction - for (size_t i = 0; i < vertexes.size(); ++i) - { - m_DebugOverlayShortPathLines.emplace_back(); - m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1); - - float x = vertexes[i].p.X.ToFloat(); - float z = vertexes[i].p.Y.ToFloat(); - - float a0 = 0, a1 = 0; - // Get arc start/end angles depending on quadrant (if any) - if (vertexes[i].quadInward == QUADRANT_BL) { a0 = -0.25f; a1 = 0.50f; } - else if (vertexes[i].quadInward == QUADRANT_TR) { a0 = 0.25f; a1 = 1.00f; } - else if (vertexes[i].quadInward == QUADRANT_TL) { a0 = -0.50f; a1 = 0.25f; } - else if (vertexes[i].quadInward == QUADRANT_BR) { a0 = 0.00f; a1 = 0.75f; } - - if (a0 == a1) - SimRender::ConstructCircleOnGround(GetSimContext(), x, z, 0.5f, - m_DebugOverlayShortPathLines.back(), true); - else - SimRender::ConstructClosedArcOnGround(GetSimContext(), x, z, 0.5f, - a0 * ((float)M_PI*2.0f), a1 * ((float)M_PI*2.0f), - m_DebugOverlayShortPathLines.back(), true); - } - - // Render the edges - for (size_t i = 0; i < edges.size(); ++i) - { - m_DebugOverlayShortPathLines.emplace_back(); - m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); - std::vector xz; - PUSH_POINT(edges[i].p0); - PUSH_POINT(edges[i].p1); - - // Add an arrowhead to indicate the direction - CFixedVector2D d = edges[i].p1 - edges[i].p0; - d.Normalize(fixed::FromInt(1)/8); - CFixedVector2D p2 = edges[i].p1 - d*2; - CFixedVector2D p3 = p2 + d.Perpendicular(); - CFixedVector2D p4 = p2 - d.Perpendicular(); - PUSH_POINT(p3); - PUSH_POINT(p4); - PUSH_POINT(edges[i].p1); - - SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); - } -#undef PUSH_POINT - - // Render the axis-aligned squares - for (size_t i = 0; i < edgeSquares.size(); ++i) - { - m_DebugOverlayShortPathLines.push_back(SOverlayLine()); - m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); - std::vector xz; - Square s = edgeSquares[i]; - xz.push_back(s.p0.X.ToFloat()); - xz.push_back(s.p0.Y.ToFloat()); - xz.push_back(s.p0.X.ToFloat()); - xz.push_back(s.p1.Y.ToFloat()); - xz.push_back(s.p1.X.ToFloat()); - xz.push_back(s.p1.Y.ToFloat()); - xz.push_back(s.p1.X.ToFloat()); - xz.push_back(s.p0.Y.ToFloat()); - xz.push_back(s.p0.X.ToFloat()); - xz.push_back(s.p0.Y.ToFloat()); - SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); - } - } - - // Do an A* search over the vertex/visibility graph: - - // Since we are just measuring Euclidean distance the heuristic is admissible, - // so we never have to re-examine a node once it's been moved to the closed set. - - // To save time in common cases, we don't precompute a graph of valid edges between vertexes; - // we do it lazily instead. When the search algorithm reaches a vertex, we examine every other - // vertex and see if we can reach it without hitting any collision edges, and ignore the ones - // we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked - // as closed), we won't be doing any redundant visibility computations. - - PROFILE_START("Short pathfinding - A*"); - - VertexPriorityQueue open; - VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h }; - open.push(qiStart); - - u16 idBest = START_VERTEX_ID; - fixed hBest = start.h; - - while (!open.empty()) - { - // Move best tile from open to closed - VertexPriorityQueue::Item curr = open.pop(); - vertexes[curr.id].status = Vertex::CLOSED; - - // If we've reached the destination, stop - if (curr.id == GOAL_VERTEX_ID) - { - idBest = curr.id; - break; - } - - // Sort the edges by distance in order to check those first that have a high probability of blocking a ray. - // The heuristic based on distance is very rough, especially for squares that are further away; - // we're also only really interested in the closest squares since they are the only ones that block a lot of rays. - // Thus we only do a partial sort; the threshold is just a somewhat reasonable value. - if (edgeSquares.size() > 8) - std::partial_sort(edgeSquares.begin(), edgeSquares.begin() + 8, edgeSquares.end(), SquareSort(vertexes[curr.id].p)); - - edgesUnaligned.clear(); - edgesLeft.clear(); - edgesRight.clear(); - edgesBottom.clear(); - edgesTop.clear(); - SplitAAEdges(vertexes[curr.id].p, edges, edgeSquares, edgesUnaligned, edgesLeft, edgesRight, edgesBottom, edgesTop); - - // Check the lines to every other vertex - for (size_t n = 0; n < vertexes.size(); ++n) - { - if (vertexes[n].status == Vertex::CLOSED) - continue; - - // If this is the magical goal vertex, move it to near the current vertex - CFixedVector2D npos; - if (n == GOAL_VERTEX_ID) - { - npos = goal.NearestPointOnGoal(vertexes[curr.id].p); - - // To prevent integer overflows later on, we need to ensure all vertexes are - // 'close' to the source. The goal might be far away (not a good idea but - // sometimes it happens), so clamp it to the current search range - npos.X = clamp(npos.X, rangeXMin, rangeXMax); - npos.Y = clamp(npos.Y, rangeZMin, rangeZMax); - } - else - npos = vertexes[n].p; - - // Work out which quadrant(s) we're approaching the new vertex from - u8 quad = 0; - if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL; - if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR; - if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL; - if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BR; - - // Check that the new vertex is in the right quadrant for the old vertex - if (!(vertexes[curr.id].quadOutward & quad)) - { - // Hack: Always head towards the goal if possible, to avoid missing it if it's - // inside another unit - if (n != GOAL_VERTEX_ID) - continue; - } - - bool visible = - CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) && - CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) && - CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) && - CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) && - CheckVisibility(vertexes[curr.id].p, npos, edgesUnaligned); - - /* - // Render the edges that we examine - m_DebugOverlayShortPathLines.push_back(SOverlayLine()); - m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); - std::vector xz; - xz.push_back(vertexes[curr.id].p.X.ToFloat()); - xz.push_back(vertexes[curr.id].p.Y.ToFloat()); - xz.push_back(npos.X.ToFloat()); - xz.push_back(npos.Y.ToFloat()); - SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false); - */ - - if (visible) - { - fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length(); - - // If this is a new tile, compute the heuristic distance - if (vertexes[n].status == Vertex::UNEXPLORED) - { - // Add it to the open list: - vertexes[n].status = Vertex::OPEN; - vertexes[n].g = g; - vertexes[n].h = goal.DistanceToPoint(npos); - vertexes[n].pred = curr.id; - - // If this is an axis-aligned shape, the path must continue in the same quadrant - // direction (but not go into the inside of the shape). - // Hack: If we started *inside* a shape then perhaps headed to its corner (e.g. the unit - // was very near another unit), don't restrict further pathing. - if (vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8))) - vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF; - - if (n == GOAL_VERTEX_ID) - vertexes[n].p = npos; // remember the new best goal position - - VertexPriorityQueue::Item t = { (u16)n, g + vertexes[n].h, vertexes[n].h }; - open.push(t); - - // Remember the heuristically best vertex we've seen so far, in case we never actually reach the target - if (vertexes[n].h < hBest) - { - idBest = (u16)n; - hBest = vertexes[n].h; - } - } - else // must be OPEN - { - // If we've already seen this tile, and the new path to this tile does not have a - // better cost, then stop now - if (g >= vertexes[n].g) - continue; - - // Otherwise, we have a better path, so replace the old one with the new cost/parent - fixed gprev = vertexes[n].g; - vertexes[n].g = g; - vertexes[n].pred = curr.id; - - // If this is an axis-aligned shape, the path must continue in the same quadrant - // direction (but not go into the inside of the shape). - if (vertexes[n].quadInward) - vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF; - - if (n == GOAL_VERTEX_ID) - vertexes[n].p = npos; // remember the new best goal position - - open.promote((u16)n, gprev + vertexes[n].h, g + vertexes[n].h, vertexes[n].h); - } - } - } - } - - // Reconstruct the path (in reverse) - for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred) - path.m_Waypoints.emplace_back(Waypoint{ vertexes[id].p.X, vertexes[id].p.Y }); - - PROFILE_END("Short pathfinding - A*"); -} Index: ps/trunk/source/simulation2/components/ICmpPathfinder.h =================================================================== --- ps/trunk/source/simulation2/components/ICmpPathfinder.h +++ ps/trunk/source/simulation2/components/ICmpPathfinder.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2017 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -114,7 +114,7 @@ * 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 void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) = 0; + virtual WaypointPath ComputeShortPath(const AsyncShortPathRequest& request) const = 0; /** * Asynchronous version of ComputeShortPath (using ControlGroupObstructionFilter). Index: ps/trunk/source/simulation2/components/tests/test_Pathfinder.h =================================================================== --- ps/trunk/source/simulation2/components/tests/test_Pathfinder.h +++ ps/trunk/source/simulation2/components/tests/test_Pathfinder.h @@ -19,7 +19,6 @@ #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/components/ICmpPathfinder.h" -#include "simulation2/components/CCmpPathfinder_Common.h" #include "graphics/MapReader.h" #include "graphics/Terrain.h" @@ -214,10 +213,8 @@ cmpObstructionMan->AddUnitShape(INVALID_ENTITY, x, z, fixed::FromInt(2), 0, INVALID_ENTITY); } - NullObstructionFilter filter; PathGoal goal = { PathGoal::POINT, range, range }; - WaypointPath path; - cmpPathfinder->ComputeShortPath(filter, range/3, range/3, fixed::FromInt(2), range, goal, 0, path); + WaypointPath path = cmpPathfinder->ComputeShortPath(AsyncShortPathRequest{ 0, range/3, range/3, fixed::FromInt(2), range, goal, 0, false, 0, 0 }); for (size_t i = 0; i < path.m_Waypoints.size(); ++i) printf("# %d: %f %f\n", (int)i, path.m_Waypoints[i].x.ToFloat(), path.m_Waypoints[i].z.ToFloat()); } Index: ps/trunk/source/simulation2/helpers/Pathfinding.h =================================================================== --- ps/trunk/source/simulation2/helpers/Pathfinding.h +++ ps/trunk/source/simulation2/helpers/Pathfinding.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2016 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -21,6 +21,7 @@ #include "maths/MathUtil.h" #include "ps/CLogger.h" +#include "simulation2/system/Entity.h" #include "simulation2/system/ParamNode.h" #include "graphics/Terrain.h" #include "Grid.h" @@ -28,6 +29,30 @@ typedef u16 pass_class_t; +struct AsyncLongPathRequest +{ + u32 ticket; + entity_pos_t x0; + entity_pos_t z0; + PathGoal goal; + pass_class_t passClass; + entity_id_t notify; +}; + +struct AsyncShortPathRequest +{ + u32 ticket; + entity_pos_t x0; + entity_pos_t z0; + entity_pos_t clearance; + entity_pos_t range; + PathGoal goal; + pass_class_t passClass; + bool avoidMovingUnits; + entity_id_t group; + entity_id_t notify; +}; + struct Waypoint { entity_pos_t x, z; Index: ps/trunk/source/simulation2/helpers/VertexPathfinder.h =================================================================== --- ps/trunk/source/simulation2/helpers/VertexPathfinder.h +++ ps/trunk/source/simulation2/helpers/VertexPathfinder.h @@ -0,0 +1,120 @@ +/* Copyright (C) 2019 Wildfire Games. + * This file is part of 0 A.D. + * + * 0 A.D. is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * 0 A.D. is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with 0 A.D. If not, see . + */ + +#ifndef INCLUDED_VERTEXPATHFINDER +#define INCLUDED_VERTEXPATHFINDER + +#include "graphics/Overlay.h" +#include "simulation2/helpers/Pathfinding.h" +#include "simulation2/system/CmpPtr.h" + +// A vertex around the corners of an obstruction +// (paths will be sequences of these vertexes) +struct Vertex +{ + enum + { + UNEXPLORED, + OPEN, + CLOSED, + }; + + CFixedVector2D p; + fixed g, h; + u16 pred; + u8 status; + u8 quadInward : 4; // the quadrant which is inside the shape (or NONE) + u8 quadOutward : 4; // the quadrants of the next point on the path which this vertex must be in, given 'pred' +}; + +// Obstruction edges (paths will not cross any of these). +// Defines the two points of the edge. +struct Edge +{ + CFixedVector2D p0, p1; +}; + +// Axis-aligned obstruction squares (paths will not cross any of these). +// Defines the opposing corners of an axis-aligned square +// (from which four individual edges can be trivially computed), requiring p0 <= p1 +struct Square +{ + CFixedVector2D p0, p1; +}; + +// Axis-aligned obstruction edges. +// p0 defines one end; c1 is either the X or Y coordinate of the other end, +// depending on the context in which this is used. +struct EdgeAA +{ + CFixedVector2D p0; + fixed c1; +}; + +class ICmpObstructionManager; +class CSimContext; +class SceneCollector; + +class VertexPathfinder +{ +public: + VertexPathfinder(const u16& mapSize, Grid* const & terrainOnlyGrid) : m_MapSize(mapSize), m_TerrainOnlyGrid(terrainOnlyGrid), m_DebugOverlay(false) {}; + + /** + * Compute a precise path from the given point to the goal, and return the set of waypoints. + * The path is based on the full set of obstructions that pass the filter, such that + * a unit of clearance 'clearance' will be able to follow the path with no collisions. + * The path is restricted to a box of radius 'range' from the starting point. + * Defined in CCmpPathfinder_Vertex.cpp + */ + WaypointPath ComputeShortPath(const AsyncShortPathRequest& request, CmpPtr cmpObstructionManager) const; + + void SetDebugOverlay(bool enabled) { m_DebugOverlay = enabled; } + void RenderSubmit(SceneCollector& collector); + +private: + + void DebugRenderGoal(const CSimContext& simContext, const PathGoal& goal) const; + void DebugRenderGraph(const CSimContext& simContext, const std::vector& vertexes, const std::vector& edges, const std::vector& edgeSquares) const; + void DebugRenderEdges(const CSimContext& simContext, bool visible, CFixedVector2D curr, CFixedVector2D npos) const; + + // References to the Pathfinder for convenience. + const u16& m_MapSize; + Grid* const & m_TerrainOnlyGrid; + + std::atomic m_DebugOverlay; + mutable std::vector m_DebugOverlayShortPathLines; + + // These vectors are expensive to recreate on every call, so we cache them here. + // They are made mutable to allow using them in the otherwise const ComputeShortPath. + + mutable std::vector m_EdgesUnaligned; + mutable std::vector m_EdgesLeft; + mutable std::vector m_EdgesRight; + mutable std::vector m_EdgesBottom; + mutable std::vector m_EdgesTop; + + // List of obstruction vertexes (plus start/end points); we'll try to find paths through + // the graph defined by these vertexes. + mutable std::vector m_Vertexes; + // List of collision edges - paths must never cross these. + // (Edges are one-sided so intersections are fine in one direction, but not the other direction.) + mutable std::vector m_Edges; + mutable std::vector m_EdgeSquares; // Axis-aligned squares; equivalent to 4 edges. +}; + +#endif // INCLUDED_VERTEXPATHFINDER Index: ps/trunk/source/simulation2/helpers/VertexPathfinder.cpp =================================================================== --- ps/trunk/source/simulation2/helpers/VertexPathfinder.cpp +++ ps/trunk/source/simulation2/helpers/VertexPathfinder.cpp @@ -0,0 +1,963 @@ +/* Copyright (C) 2019 Wildfire Games. + * This file is part of 0 A.D. + * + * 0 A.D. is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * 0 A.D. is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with 0 A.D. If not, see . + */ + +/** + * @file + * Vertex-based algorithm for CCmpPathfinder. + * Computes paths around the corners of rectangular obstructions. + * + * Useful search term for this algorithm: "points of visibility". + * + * Since we sometimes want to use this for avoiding moving units, there is no + * pre-computation - the whole visibility graph is effectively regenerated for + * each path, and it does A* over that graph. + * + * This scales very poorly in the number of obstructions, so it should be used + * with a limited range and not exceedingly frequently. + */ + +#include "precompiled.h" + +#include "VertexPathfinder.h" + +#include "lib/timer.h" +#include "ps/Profile.h" +#include "renderer/Scene.h" +#include "simulation2/components/ICmpObstructionManager.h" +#include "simulation2/helpers/PriorityQueue.h" +#include "simulation2/helpers/Render.h" +#include "simulation2/system/SimContext.h" + +/* Quadrant optimisation: + * (loosely based on GPG2 "Optimizing Points-of-Visibility Pathfinding") + * + * Consider the vertex ("@") at a corner of an axis-aligned rectangle ("#"): + * + * TL : TR + * : + * ####@ - - - + * ##### + * ##### + * BL ## BR + * + * The area around the vertex is split into TopLeft, BottomRight etc quadrants. + * + * If the shortest path reaches this vertex, it cannot continue to a vertex in + * the BL quadrant (it would be blocked by the shape). + * Since the shortest path is wrapped tightly around the edges of obstacles, + * if the path approached this vertex from the TL quadrant, + * it cannot continue to the TL or TR quadrants (the path could be shorter if it + * skipped this vertex). + * Therefore it must continue to a vertex in the BR quadrant (so this vertex is in + * *that* vertex's TL quadrant). + * + * That lets us significantly reduce the search space by quickly discarding vertexes + * from the wrong quadrants. + * + * (This causes badness if the path starts from inside the shape, so we add some hacks + * for that case.) + * + * (For non-axis-aligned rectangles it's harder to do this computation, so we'll + * not bother doing any discarding for those.) + */ +static const u8 QUADRANT_NONE = 0; +static const u8 QUADRANT_BL = 1; +static const u8 QUADRANT_TR = 2; +static const u8 QUADRANT_TL = 4; +static const u8 QUADRANT_BR = 8; +static const u8 QUADRANT_BLTR = QUADRANT_BL|QUADRANT_TR; +static const u8 QUADRANT_TLBR = QUADRANT_TL|QUADRANT_BR; +static const u8 QUADRANT_ALL = QUADRANT_BLTR|QUADRANT_TLBR; + +// When computing vertexes to insert into the search graph, +// add a small delta so that the vertexes of an edge don't get interpreted +// as crossing the edge (given minor numerical inaccuracies) +static const entity_pos_t EDGE_EXPAND_DELTA = entity_pos_t::FromInt(1)/16; + +/** + * Check whether a ray from 'a' to 'b' crosses any of the edges. + * (Edges are one-sided so it's only considered a cross if going from front to back.) + */ +inline static bool CheckVisibility(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) +{ + CFixedVector2D abn = (b - a).Perpendicular(); + + // Edges of general non-axis-aligned shapes + for (size_t i = 0; i < edges.size(); ++i) + { + CFixedVector2D p0 = edges[i].p0; + CFixedVector2D p1 = edges[i].p1; + + CFixedVector2D d = (p1 - p0).Perpendicular(); + + // If 'a' is behind the edge, we can't cross + fixed q = (a - p0).Dot(d); + if (q < fixed::Zero()) + continue; + + // If 'b' is in front of the edge, we can't cross + fixed r = (b - p0).Dot(d); + if (r > fixed::Zero()) + continue; + + // The ray is crossing the infinitely-extended edge from in front to behind. + // Check the finite edge is crossing the infinitely-extended ray too. + // (Given the previous tests, it can only be crossing in one direction.) + fixed s = (p0 - a).Dot(abn); + if (s > fixed::Zero()) + continue; + + fixed t = (p1 - a).Dot(abn); + if (t < fixed::Zero()) + continue; + + return false; + } + + return true; +} + +// Handle the axis-aligned shape edges separately (for performance): +// (These are specialised versions of the general unaligned edge code. +// They assume the caller has already excluded edges for which 'a' is +// on the wrong side.) + +inline static bool CheckVisibilityLeft(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) +{ + if (a.X >= b.X) + return true; + + CFixedVector2D abn = (b - a).Perpendicular(); + + for (size_t i = 0; i < edges.size(); ++i) + { + if (b.X < edges[i].p0.X) + continue; + + CFixedVector2D p0 (edges[i].p0.X, edges[i].c1); + fixed s = (p0 - a).Dot(abn); + if (s > fixed::Zero()) + continue; + + CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y); + fixed t = (p1 - a).Dot(abn); + if (t < fixed::Zero()) + continue; + + return false; + } + + return true; +} + +inline static bool CheckVisibilityRight(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) +{ + if (a.X <= b.X) + return true; + + CFixedVector2D abn = (b - a).Perpendicular(); + + for (size_t i = 0; i < edges.size(); ++i) + { + if (b.X > edges[i].p0.X) + continue; + + CFixedVector2D p0 (edges[i].p0.X, edges[i].c1); + fixed s = (p0 - a).Dot(abn); + if (s > fixed::Zero()) + continue; + + CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y); + fixed t = (p1 - a).Dot(abn); + if (t < fixed::Zero()) + continue; + + return false; + } + + return true; +} + +inline static bool CheckVisibilityBottom(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) +{ + if (a.Y >= b.Y) + return true; + + CFixedVector2D abn = (b - a).Perpendicular(); + + for (size_t i = 0; i < edges.size(); ++i) + { + if (b.Y < edges[i].p0.Y) + continue; + + CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y); + fixed s = (p0 - a).Dot(abn); + if (s > fixed::Zero()) + continue; + + CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y); + fixed t = (p1 - a).Dot(abn); + if (t < fixed::Zero()) + continue; + + return false; + } + + return true; +} + +inline static bool CheckVisibilityTop(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& edges) +{ + if (a.Y <= b.Y) + return true; + + CFixedVector2D abn = (b - a).Perpendicular(); + + for (size_t i = 0; i < edges.size(); ++i) + { + if (b.Y > edges[i].p0.Y) + continue; + + CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y); + fixed s = (p0 - a).Dot(abn); + if (s > fixed::Zero()) + continue; + + CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y); + fixed t = (p1 - a).Dot(abn); + if (t < fixed::Zero()) + continue; + + return false; + } + + return true; +} + +typedef PriorityQueueHeap VertexPriorityQueue; + +/** + * Add edges and vertexes to represent the boundaries between passable and impassable + * navcells (for impassable terrain). + * Navcells i0 <= i <= i1, j0 <= j <= j1 will be considered. + */ +static void AddTerrainEdges(std::vector& edges, std::vector& vertexes, + int i0, int j0, int i1, int j1, + pass_class_t passClass, const Grid& grid) +{ + + // Clamp the coordinates so we won't attempt to sample outside of the grid. + // (This assumes the outermost ring of navcells (which are always impassable) + // won't have a boundary with any passable navcells. TODO: is that definitely + // safe enough?) + + i0 = clamp(i0, 1, grid.m_W-2); + j0 = clamp(j0, 1, grid.m_H-2); + i1 = clamp(i1, 1, grid.m_W-2); + j1 = clamp(j1, 1, grid.m_H-2); + + for (int j = j0; j <= j1; ++j) + { + for (int i = i0; i <= i1; ++i) + { + if (IS_PASSABLE(grid.get(i, j), passClass)) + continue; + + if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i+1, j+1), passClass)) + { + Vertex vert; + vert.status = Vertex::UNEXPLORED; + vert.quadOutward = QUADRANT_ALL; + vert.quadInward = QUADRANT_BL; + vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); + vertexes.push_back(vert); + } + + if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i-1, j+1), passClass)) + { + Vertex vert; + vert.status = Vertex::UNEXPLORED; + vert.quadOutward = QUADRANT_ALL; + vert.quadInward = QUADRANT_BR; + vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); + vertexes.push_back(vert); + } + + if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i+1, j-1), passClass)) + { + Vertex vert; + vert.status = Vertex::UNEXPLORED; + vert.quadOutward = QUADRANT_ALL; + vert.quadInward = QUADRANT_TL; + vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); + vertexes.push_back(vert); + } + + if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i-1, j-1), passClass)) + { + Vertex vert; + vert.status = Vertex::UNEXPLORED; + vert.quadOutward = QUADRANT_ALL; + vert.quadInward = QUADRANT_TR; + vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE); + vertexes.push_back(vert); + } + } + } + + // XXX rewrite this stuff + std::vector segmentsR; + std::vector segmentsL; + for (int j = j0; j < j1; ++j) + { + segmentsR.clear(); + segmentsL.clear(); + for (int i = i0; i <= i1; ++i) + { + bool a = IS_PASSABLE(grid.get(i, j+1), passClass); + bool b = IS_PASSABLE(grid.get(i, j), passClass); + if (a && !b) + segmentsL.push_back(i); + if (b && !a) + segmentsR.push_back(i); + } + + if (!segmentsR.empty()) + { + segmentsR.push_back(0); // sentinel value to simplify the loop + u16 ia = segmentsR[0]; + u16 ib = ia + 1; + for (size_t n = 1; n < segmentsR.size(); ++n) + { + if (segmentsR[n] == ib) + ++ib; + else + { + CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); + CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); + edges.emplace_back(Edge{ v0, v1 }); + + ia = segmentsR[n]; + ib = ia + 1; + } + } + } + + if (!segmentsL.empty()) + { + segmentsL.push_back(0); // sentinel value to simplify the loop + u16 ia = segmentsL[0]; + u16 ib = ia + 1; + for (size_t n = 1; n < segmentsL.size(); ++n) + { + if (segmentsL[n] == ib) + ++ib; + else + { + CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); + CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE); + edges.emplace_back(Edge{ v0, v1 }); + + ia = segmentsL[n]; + ib = ia + 1; + } + } + } + } + std::vector segmentsU; + std::vector segmentsD; + for (int i = i0; i < i1; ++i) + { + segmentsU.clear(); + segmentsD.clear(); + for (int j = j0; j <= j1; ++j) + { + bool a = IS_PASSABLE(grid.get(i+1, j), passClass); + bool b = IS_PASSABLE(grid.get(i, j), passClass); + if (a && !b) + segmentsU.push_back(j); + if (b && !a) + segmentsD.push_back(j); + } + + if (!segmentsU.empty()) + { + segmentsU.push_back(0); // sentinel value to simplify the loop + u16 ja = segmentsU[0]; + u16 jb = ja + 1; + for (size_t n = 1; n < segmentsU.size(); ++n) + { + if (segmentsU[n] == jb) + ++jb; + else + { + CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE); + CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE); + edges.emplace_back(Edge{ v0, v1 }); + + ja = segmentsU[n]; + jb = ja + 1; + } + } + } + + if (!segmentsD.empty()) + { + segmentsD.push_back(0); // sentinel value to simplify the loop + u16 ja = segmentsD[0]; + u16 jb = ja + 1; + for (size_t n = 1; n < segmentsD.size(); ++n) + { + if (segmentsD[n] == jb) + ++jb; + else + { + CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE); + CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE); + edges.emplace_back(Edge{ v0, v1 }); + + ja = segmentsD[n]; + jb = ja + 1; + } + } + } + } +} + +static void SplitAAEdges(const CFixedVector2D& a, + const std::vector& edges, + const std::vector& squares, + std::vector& edgesUnaligned, + std::vector& edgesLeft, std::vector& edgesRight, + std::vector& edgesBottom, std::vector& edgesTop) +{ + + for (const Square& square : squares) + { + if (a.X <= square.p0.X) + edgesLeft.emplace_back(EdgeAA{ square.p0, square.p1.Y }); + if (a.X >= square.p1.X) + edgesRight.emplace_back(EdgeAA{ square.p1, square.p0.Y }); + if (a.Y <= square.p0.Y) + edgesBottom.emplace_back(EdgeAA{ square.p0, square.p1.X }); + if (a.Y >= square.p1.Y) + edgesTop.emplace_back(EdgeAA{ square.p1, square.p0.X }); + } + + for (const Edge& edge : edges) + { + if (edge.p0.X == edge.p1.X) + { + if (edge.p1.Y < edge.p0.Y) + { + if (!(a.X <= edge.p0.X)) + continue; + edgesLeft.emplace_back(EdgeAA{ edge.p1, edge.p0.Y }); + } + else + { + if (!(a.X >= edge.p0.X)) + continue; + edgesRight.emplace_back(EdgeAA{ edge.p1, edge.p0.Y }); + } + } + else if (edge.p0.Y == edge.p1.Y) + { + if (edge.p0.X < edge.p1.X) + { + if (!(a.Y <= edge.p0.Y)) + continue; + edgesBottom.emplace_back(EdgeAA{ edge.p0, edge.p1.X }); + } + else + { + if (!(a.Y >= edge.p0.Y)) + continue; + edgesTop.emplace_back(EdgeAA{ edge.p0, edge.p1.X }); + } + } + else + edgesUnaligned.push_back(edge); + } +} + +/** + * Functor for sorting edge-squares by approximate proximity to a fixed point. + */ +struct SquareSort +{ + CFixedVector2D src; + SquareSort(CFixedVector2D src) : src(src) { } + bool operator()(const Square& a, const Square& b) const + { + if ((a.p0 - src).CompareLength(b.p0 - src) < 0) + return true; + return false; + } +}; + +WaypointPath VertexPathfinder::ComputeShortPath(const AsyncShortPathRequest& request, CmpPtr cmpObstructionManager) const +{ + PROFILE2("ComputeShortPath"); + + DebugRenderGoal(cmpObstructionManager->GetSimContext(), request.goal); + + // Create impassable edges at the max-range boundary, so we can't escape the region + // where we're meant to be searching + fixed rangeXMin = request.x0 - request.range; + fixed rangeXMax = request.x0 + request.range; + fixed rangeZMin = request.z0 - request.range; + fixed rangeZMax = request.z0 + request.range; + + // we don't actually add the "search space" edges as edges, since we may want to cross them + // in some cases (such as if we need to go around an obstruction that's partly out of the search range) + + + // Add the start point to the graph + CFixedVector2D posStart(request.x0, request.z0); + fixed hStart = (posStart - request.goal.NearestPointOnGoal(posStart)).Length(); + Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL }; + m_Vertexes.push_back(start); + const size_t START_VERTEX_ID = 0; + + // Add the goal vertex to the graph. + // Since the goal isn't always a point, this a special magic virtual vertex which moves around - whenever + // we look at it from another vertex, it is moved to be the closest point on the goal shape to that vertex. + Vertex end = { CFixedVector2D(request.goal.x, request.goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL }; + m_Vertexes.push_back(end); + const size_t GOAL_VERTEX_ID = 1; + + // Find all the obstruction squares that might affect us + std::vector squares; + size_t staticShapesNb = 0; + ControlGroupMovementObstructionFilter filter(request.avoidMovingUnits, request.group); + cmpObstructionManager->GetStaticObstructionsInRange(filter, rangeXMin - request.clearance, rangeZMin - request.clearance, rangeXMax + request.clearance, rangeZMax + request.clearance, squares); + staticShapesNb = squares.size(); + cmpObstructionManager->GetUnitObstructionsInRange(filter, rangeXMin - request.clearance, rangeZMin - request.clearance, rangeXMax + request.clearance, rangeZMax + request.clearance, squares); + + // Change array capacities to reduce reallocations + m_Vertexes.reserve(m_Vertexes.size() + squares.size()*4); + m_EdgeSquares.reserve(m_EdgeSquares.size() + squares.size()); // (assume most squares are AA) + + entity_pos_t pathfindClearance = request.clearance; + + // Convert each obstruction square into collision edges and search graph vertexes + for (size_t i = 0; i < squares.size(); ++i) + { + CFixedVector2D center(squares[i].x, squares[i].z); + CFixedVector2D u = squares[i].u; + CFixedVector2D v = squares[i].v; + + if (i >= staticShapesNb) + pathfindClearance = request.clearance - entity_pos_t::FromInt(1)/2; + + // Expand the vertexes by the moving unit's collision radius, to find the + // closest we can get to it + + CFixedVector2D hd0(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA, squares[i].hh + pathfindClearance + EDGE_EXPAND_DELTA); + CFixedVector2D hd1(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA, -(squares[i].hh + pathfindClearance + EDGE_EXPAND_DELTA)); + + // Check whether this is an axis-aligned square + bool aa = (u.X == fixed::FromInt(1) && u.Y == fixed::Zero() && v.X == fixed::Zero() && v.Y == fixed::FromInt(1)); + + Vertex vert; + vert.status = Vertex::UNEXPLORED; + vert.quadInward = QUADRANT_NONE; + vert.quadOutward = QUADRANT_ALL; + vert.p.X = center.X - hd0.Dot(u); vert.p.Y = center.Y + hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_BR; m_Vertexes.push_back(vert); + if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; + if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; + if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; + if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; + vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_TR; m_Vertexes.push_back(vert); + if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; + if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; + if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; + if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; + vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_TL; m_Vertexes.push_back(vert); + if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; + if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; + if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; + if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; + vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; m_Vertexes.push_back(vert); + if (vert.p.X < rangeXMin) rangeXMin = vert.p.X; + if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y; + if (vert.p.X > rangeXMax) rangeXMax = vert.p.X; + if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y; + + // Add the edges: + + CFixedVector2D h0(squares[i].hw + pathfindClearance, squares[i].hh + pathfindClearance); + CFixedVector2D h1(squares[i].hw + pathfindClearance, -(squares[i].hh + pathfindClearance)); + + CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v)); + CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v)); + CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v)); + CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v)); + if (aa) + m_EdgeSquares.emplace_back(Square{ ev1, ev3 }); + else + { + m_Edges.emplace_back(Edge{ ev0, ev1 }); + m_Edges.emplace_back(Edge{ ev1, ev2 }); + m_Edges.emplace_back(Edge{ ev2, ev3 }); + m_Edges.emplace_back(Edge{ ev3, ev0 }); + } + + // TODO: should clip out vertexes and edges that are outside the range, + // to reduce the search space + } + + // Add terrain obstructions + { + u16 i0, j0, i1, j1; + Pathfinding::NearestNavcell(rangeXMin, rangeZMin, i0, j0, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); + Pathfinding::NearestNavcell(rangeXMax, rangeZMax, i1, j1, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); + AddTerrainEdges(m_Edges, m_Vertexes, i0, j0, i1, j1, request.passClass, *m_TerrainOnlyGrid); + } + + // Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable) + for (size_t i = 0; i < m_EdgeSquares.size(); ++i) + { + // If the start point is inside the square, ignore it + if (start.p.X >= m_EdgeSquares[i].p0.X && + start.p.Y >= m_EdgeSquares[i].p0.Y && + start.p.X <= m_EdgeSquares[i].p1.X && + start.p.Y <= m_EdgeSquares[i].p1.Y) + continue; + + // Remove every non-start/goal vertex that is inside an edgeSquare; + // since remove() would be inefficient, just mark it as closed instead. + for (size_t j = 2; j < m_Vertexes.size(); ++j) + if (m_Vertexes[j].p.X >= m_EdgeSquares[i].p0.X && + m_Vertexes[j].p.Y >= m_EdgeSquares[i].p0.Y && + m_Vertexes[j].p.X <= m_EdgeSquares[i].p1.X && + m_Vertexes[j].p.Y <= m_EdgeSquares[i].p1.Y) + m_Vertexes[j].status = Vertex::CLOSED; + } + + ENSURE(m_Vertexes.size() < 65536); // We store array indexes as u16. + + DebugRenderGraph(cmpObstructionManager->GetSimContext(), m_Vertexes, m_Edges, m_EdgeSquares); + + // Do an A* search over the vertex/visibility graph: + + // Since we are just measuring Euclidean distance the heuristic is admissible, + // so we never have to re-examine a node once it's been moved to the closed set. + + // To save time in common cases, we don't precompute a graph of valid edges between vertexes; + // we do it lazily instead. When the search algorithm reaches a vertex, we examine every other + // vertex and see if we can reach it without hitting any collision edges, and ignore the ones + // we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked + // as closed), we won't be doing any redundant visibility computations. + + VertexPriorityQueue open; + VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h }; + open.push(qiStart); + + u16 idBest = START_VERTEX_ID; + fixed hBest = start.h; + + while (!open.empty()) + { + // Move best tile from open to closed + VertexPriorityQueue::Item curr = open.pop(); + m_Vertexes[curr.id].status = Vertex::CLOSED; + + // If we've reached the destination, stop + if (curr.id == GOAL_VERTEX_ID) + { + idBest = curr.id; + break; + } + + // Sort the edges by distance in order to check those first that have a high probability of blocking a ray. + // The heuristic based on distance is very rough, especially for squares that are further away; + // we're also only really interested in the closest squares since they are the only ones that block a lot of rays. + // Thus we only do a partial sort; the threshold is just a somewhat reasonable value. + if (m_EdgeSquares.size() > 8) + std::partial_sort(m_EdgeSquares.begin(), m_EdgeSquares.begin() + 8, m_EdgeSquares.end(), SquareSort(m_Vertexes[curr.id].p)); + + m_EdgesUnaligned.clear(); + m_EdgesLeft.clear(); + m_EdgesRight.clear(); + m_EdgesBottom.clear(); + m_EdgesTop.clear(); + SplitAAEdges(m_Vertexes[curr.id].p, m_Edges, m_EdgeSquares, m_EdgesUnaligned, m_EdgesLeft, m_EdgesRight, m_EdgesBottom, m_EdgesTop); + + // Check the lines to every other vertex + for (size_t n = 0; n < m_Vertexes.size(); ++n) + { + if (m_Vertexes[n].status == Vertex::CLOSED) + continue; + + // If this is the magical goal vertex, move it to near the current vertex + CFixedVector2D npos; + if (n == GOAL_VERTEX_ID) + { + npos = request.goal.NearestPointOnGoal(m_Vertexes[curr.id].p); + + // To prevent integer overflows later on, we need to ensure all vertexes are + // 'close' to the source. The goal might be far away (not a good idea but + // sometimes it happens), so clamp it to the current search range + npos.X = clamp(npos.X, rangeXMin, rangeXMax); + npos.Y = clamp(npos.Y, rangeZMin, rangeZMax); + } + else + npos = m_Vertexes[n].p; + + // Work out which quadrant(s) we're approaching the new vertex from + u8 quad = 0; + if (m_Vertexes[curr.id].p.X <= npos.X && m_Vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL; + if (m_Vertexes[curr.id].p.X >= npos.X && m_Vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR; + if (m_Vertexes[curr.id].p.X <= npos.X && m_Vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL; + if (m_Vertexes[curr.id].p.X >= npos.X && m_Vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BR; + + // Check that the new vertex is in the right quadrant for the old vertex + if (!(m_Vertexes[curr.id].quadOutward & quad)) + { + // Hack: Always head towards the goal if possible, to avoid missing it if it's + // inside another unit + if (n != GOAL_VERTEX_ID) + continue; + } + + bool visible = + CheckVisibilityLeft(m_Vertexes[curr.id].p, npos, m_EdgesLeft) && + CheckVisibilityRight(m_Vertexes[curr.id].p, npos, m_EdgesRight) && + CheckVisibilityBottom(m_Vertexes[curr.id].p, npos, m_EdgesBottom) && + CheckVisibilityTop(m_Vertexes[curr.id].p, npos, m_EdgesTop) && + CheckVisibility(m_Vertexes[curr.id].p, npos, m_EdgesUnaligned); + + // Render the edges that we examine. + DebugRenderEdges(cmpObstructionManager->GetSimContext(), visible, m_Vertexes[curr.id].p, npos); + + if (visible) + { + fixed g = m_Vertexes[curr.id].g + (m_Vertexes[curr.id].p - npos).Length(); + + // If this is a new tile, compute the heuristic distance + if (m_Vertexes[n].status == Vertex::UNEXPLORED) + { + // Add it to the open list: + m_Vertexes[n].status = Vertex::OPEN; + m_Vertexes[n].g = g; + m_Vertexes[n].h = request.goal.DistanceToPoint(npos); + m_Vertexes[n].pred = curr.id; + + // If this is an axis-aligned shape, the path must continue in the same quadrant + // direction (but not go into the inside of the shape). + // Hack: If we started *inside* a shape then perhaps headed to its corner (e.g. the unit + // was very near another unit), don't restrict further pathing. + if (m_Vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8))) + m_Vertexes[n].quadOutward = ((m_Vertexes[n].quadInward) & quad) & 0xF; + + if (n == GOAL_VERTEX_ID) + m_Vertexes[n].p = npos; // remember the new best goal position + + VertexPriorityQueue::Item t = { (u16)n, g + m_Vertexes[n].h, m_Vertexes[n].h }; + open.push(t); + + // Remember the heuristically best vertex we've seen so far, in case we never actually reach the target + if (m_Vertexes[n].h < hBest) + { + idBest = (u16)n; + hBest = m_Vertexes[n].h; + } + } + else // must be OPEN + { + // If we've already seen this tile, and the new path to this tile does not have a + // better cost, then stop now + if (g >= m_Vertexes[n].g) + continue; + + // Otherwise, we have a better path, so replace the old one with the new cost/parent + fixed gprev = m_Vertexes[n].g; + m_Vertexes[n].g = g; + m_Vertexes[n].pred = curr.id; + + // If this is an axis-aligned shape, the path must continue in the same quadrant + // direction (but not go into the inside of the shape). + if (m_Vertexes[n].quadInward) + m_Vertexes[n].quadOutward = ((m_Vertexes[n].quadInward) & quad) & 0xF; + + if (n == GOAL_VERTEX_ID) + m_Vertexes[n].p = npos; // remember the new best goal position + + open.promote((u16)n, gprev + m_Vertexes[n].h, g + m_Vertexes[n].h, m_Vertexes[n].h); + } + } + } + } + + // Reconstruct the path (in reverse) + WaypointPath path; + for (u16 id = idBest; id != START_VERTEX_ID; id = m_Vertexes[id].pred) + path.m_Waypoints.emplace_back(Waypoint{ m_Vertexes[id].p.X, m_Vertexes[id].p.Y }); + + + m_Edges.clear(); + m_EdgeSquares.clear(); + m_Vertexes.clear(); + + m_EdgesUnaligned.clear(); + m_EdgesLeft.clear(); + m_EdgesRight.clear(); + m_EdgesBottom.clear(); + m_EdgesTop.clear(); + + return path; +} + +void VertexPathfinder::DebugRenderGoal(const CSimContext& simContext, const PathGoal& goal) const +{ + if (!m_DebugOverlay) + return; + + m_DebugOverlayShortPathLines.clear(); + + // Render the goal shape + m_DebugOverlayShortPathLines.push_back(SOverlayLine()); + m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1); + switch (goal.type) + { + case PathGoal::POINT: + { + SimRender::ConstructCircleOnGround(simContext, goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true); + break; + } + case PathGoal::CIRCLE: + case PathGoal::INVERTED_CIRCLE: + { + SimRender::ConstructCircleOnGround(simContext, goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true); + break; + } + case PathGoal::SQUARE: + case PathGoal::INVERTED_SQUARE: + { + float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat()); + SimRender::ConstructSquareOnGround(simContext, goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true); + break; + } + } +} + +void VertexPathfinder::DebugRenderGraph(const CSimContext& simContext, const std::vector& vertexes, const std::vector& edges, const std::vector& edgeSquares) const +{ + if (!m_DebugOverlay) + return; + +#define PUSH_POINT(p) STMT(xz.push_back(p.X.ToFloat()); xz.push_back(p.Y.ToFloat())) + // Render the vertexes as little Pac-Man shapes to indicate quadrant direction + for (size_t i = 0; i < vertexes.size(); ++i) + { + m_DebugOverlayShortPathLines.emplace_back(); + m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1); + + float x = vertexes[i].p.X.ToFloat(); + float z = vertexes[i].p.Y.ToFloat(); + + float a0 = 0, a1 = 0; + // Get arc start/end angles depending on quadrant (if any) + if (vertexes[i].quadInward == QUADRANT_BL) { a0 = -0.25f; a1 = 0.50f; } + else if (vertexes[i].quadInward == QUADRANT_TR) { a0 = 0.25f; a1 = 1.00f; } + else if (vertexes[i].quadInward == QUADRANT_TL) { a0 = -0.50f; a1 = 0.25f; } + else if (vertexes[i].quadInward == QUADRANT_BR) { a0 = 0.00f; a1 = 0.75f; } + + if (a0 == a1) + SimRender::ConstructCircleOnGround(simContext, x, z, 0.5f, + m_DebugOverlayShortPathLines.back(), true); + else + SimRender::ConstructClosedArcOnGround(simContext, x, z, 0.5f, + a0 * ((float)M_PI*2.0f), a1 * ((float)M_PI*2.0f), + m_DebugOverlayShortPathLines.back(), true); + } + + // Render the edges + for (size_t i = 0; i < edges.size(); ++i) + { + m_DebugOverlayShortPathLines.emplace_back(); + m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); + std::vector xz; + PUSH_POINT(edges[i].p0); + PUSH_POINT(edges[i].p1); + + // Add an arrowhead to indicate the direction + CFixedVector2D d = edges[i].p1 - edges[i].p0; + d.Normalize(fixed::FromInt(1)/8); + CFixedVector2D p2 = edges[i].p1 - d*2; + CFixedVector2D p3 = p2 + d.Perpendicular(); + CFixedVector2D p4 = p2 - d.Perpendicular(); + PUSH_POINT(p3); + PUSH_POINT(p4); + PUSH_POINT(edges[i].p1); + + SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), true); + } +#undef PUSH_POINT + + // Render the axis-aligned squares + for (size_t i = 0; i < edgeSquares.size(); ++i) + { + m_DebugOverlayShortPathLines.push_back(SOverlayLine()); + m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); + std::vector xz; + Square s = edgeSquares[i]; + xz.push_back(s.p0.X.ToFloat()); + xz.push_back(s.p0.Y.ToFloat()); + xz.push_back(s.p0.X.ToFloat()); + xz.push_back(s.p1.Y.ToFloat()); + xz.push_back(s.p1.X.ToFloat()); + xz.push_back(s.p1.Y.ToFloat()); + xz.push_back(s.p1.X.ToFloat()); + xz.push_back(s.p0.Y.ToFloat()); + xz.push_back(s.p0.X.ToFloat()); + xz.push_back(s.p0.Y.ToFloat()); + SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), true); + } +} + +void VertexPathfinder::DebugRenderEdges(const CSimContext& UNUSED(simContext), bool UNUSED(visible), CFixedVector2D UNUSED(curr), CFixedVector2D UNUSED(npos)) const +{ + if (!m_DebugOverlay) + return; + + // Disabled by default. + /* + m_DebugOverlayShortPathLines.push_back(SOverlayLine()); + m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); + m_DebugOverlayShortPathLines.push_back(SOverlayLine()); + m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); + std::vector xz; + xz.push_back(curr.X.ToFloat()); + xz.push_back(curr.Y.ToFloat()); + xz.push_back(npos.X.ToFloat()); + xz.push_back(npos.Y.ToFloat()); + SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false); + SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false); + */ +} + +void VertexPathfinder::RenderSubmit(SceneCollector& collector) +{ + if (!m_DebugOverlay) + return; + + for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i) + collector.Submit(&m_DebugOverlayShortPathLines[i]); +}