From 809f297707758330ed3d1528256f86cf613060ba Mon Sep 17 00:00:00 2001 From: wraitii Date: Wed, 8 May 2019 11:53:02 +0000 Subject: [PATCH] Move the Vertex Pathfinder to its own helper class The vertex pathfinder was implemented directly in CCmpPathfinder, instead of being a separate helper like the hierarchical pathfinder or the long pathfinder. This moves it to its own helper VertexPathfinder, which gets us ready for D14 and pathfinder threading. Some struct definitions need to be moved around. Differential Revision: https://code.wildfiregames.com/D1855 This was SVN commit r22253. --- .../simulation2/components/CCmpPathfinder.cpp | 46 +- .../components/CCmpPathfinder_Common.h | 118 +---- .../simulation2/components/ICmpPathfinder.h | 4 +- .../components/tests/test_Pathfinder.h | 5 +- source/simulation2/helpers/Pathfinding.h | 27 +- .../VertexPathfinder.cpp} | 456 ++++++++++-------- source/simulation2/helpers/VertexPathfinder.h | 120 +++++ 7 files changed, 442 insertions(+), 334 deletions(-) rename source/simulation2/{components/CCmpPathfinder_Vertex.cpp => helpers/VertexPathfinder.cpp} (69%) create mode 100755 source/simulation2/helpers/VertexPathfinder.h diff --git a/source/simulation2/components/CCmpPathfinder.cpp b/source/simulation2/components/CCmpPathfinder.cpp index 25eb7c5247..9daa2a6ce9 100644 --- a/source/simulation2/components/CCmpPathfinder.cpp +++ b/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 @@ void CCmpPathfinder::Init(const CParamNode& UNUSED(paramNode)) 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 @@ void CCmpPathfinder::Init(const CParamNode& UNUSED(paramNode)) } } +CCmpPathfinder::~CCmpPathfinder() {}; + void CCmpPathfinder::Deinit() { SetDebugOverlay(false); // cleans up memory @@ -183,12 +187,31 @@ void CCmpPathfinder::HandleMessage(const CMessage& msg, bool UNUSED(global)) 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 @@ void CCmpPathfinder::TerrainUpdateHelper(bool expandPassability/* = true */) ////////////////////////////////////////////////////////// -// 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 @@ u32 CCmpPathfinder::ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, enti 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 @@ void CCmpPathfinder::ProcessShortRequests(const std::vectorComputeShortPath(req, CmpPtr(GetSystemEntity())); CMessagePathResult msg(req.ticket, path); GetSimContext().GetComponentManager().PostMessage(req.notify, msg); } diff --git a/source/simulation2/components/CCmpPathfinder_Common.h b/source/simulation2/components/CCmpPathfinder_Common.h index 01d21a32a9..7a33af6e0b 100644 --- a/source/simulation2/components/CCmpPathfinder_Common.h +++ b/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 @@ class AtlasOverlay; #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 @@ public: componentManager.SubscribeToMessageType(MT_TurnStart); } + ~CCmpPathfinder(); + DEFAULT_COMPONENT_ALLOCATOR(Pathfinder) // Template state: @@ -158,6 +95,7 @@ public: GridUpdateInformation m_AIPathfinderDirtinessInformation; bool m_TerrainDirty; + std::unique_ptr m_VertexPathfinder; // Interface to the long-range pathfinder. LongPathfinder m_LongPathfinder; @@ -165,23 +103,6 @@ public: 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 @@ public: 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); diff --git a/source/simulation2/components/ICmpPathfinder.h b/source/simulation2/components/ICmpPathfinder.h index 7301f460dd..29474594fe 100644 --- a/source/simulation2/components/ICmpPathfinder.h +++ b/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 @@ public: * 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). diff --git a/source/simulation2/components/tests/test_Pathfinder.h b/source/simulation2/components/tests/test_Pathfinder.h index 3c45c154ff..008d512f4a 100644 --- a/source/simulation2/components/tests/test_Pathfinder.h +++ b/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 @@ public: 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()); } diff --git a/source/simulation2/helpers/Pathfinding.h b/source/simulation2/helpers/Pathfinding.h index d35d4ce841..0c0081e2f6 100644 --- a/source/simulation2/helpers/Pathfinding.h +++ b/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; diff --git a/source/simulation2/components/CCmpPathfinder_Vertex.cpp b/source/simulation2/helpers/VertexPathfinder.cpp similarity index 69% rename from source/simulation2/components/CCmpPathfinder_Vertex.cpp rename to source/simulation2/helpers/VertexPathfinder.cpp index b9214da9a5..ea8c5ac82e 100644 --- a/source/simulation2/components/CCmpPathfinder_Vertex.cpp +++ b/source/simulation2/helpers/VertexPathfinder.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2017 Wildfire Games. +/* Copyright (C) 2019 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -32,13 +32,15 @@ #include "precompiled.h" -#include "CCmpPathfinder_Common.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") @@ -257,7 +259,6 @@ static void AddTerrainEdges(std::vector& edges, std::vector& verte 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) @@ -509,87 +510,50 @@ struct SquareSort } }; -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) +WaypointPath VertexPathfinder::ComputeShortPath(const AsyncShortPathRequest& request, CmpPtr cmpObstructionManager) const { - PROFILE("ComputeShortPath"); - m_DebugOverlayShortPathLines.clear(); + PROFILE2("ComputeShortPath"); - 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 + 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 = x0 - range; - fixed rangeXMax = x0 + range; - fixed rangeZMin = z0 - range; - fixed rangeZMax = z0 + range; + 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) - // 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(); + 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 }; - vertexes.push_back(start); + 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(goal.x, goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL }; - vertexes.push_back(end); + 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 - CmpPtr cmpObstructionManager(GetSystemEntity()); std::vector squares; size_t staticShapesNb = 0; - cmpObstructionManager->GetStaticObstructionsInRange(filter, rangeXMin - clearance, rangeZMin - clearance, rangeXMax + clearance, rangeZMax + clearance, squares); + 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 - clearance, rangeZMin - clearance, rangeXMax + clearance, rangeZMax + clearance, squares); + cmpObstructionManager->GetUnitObstructionsInRange(filter, rangeXMin - request.clearance, rangeZMin - request.clearance, rangeXMax + request.clearance, rangeZMax + request.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) + 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 = clearance; + 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) @@ -599,7 +563,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, CFixedVector2D v = squares[i].v; if (i >= staticShapesNb) - pathfindClearance = clearance - entity_pos_t::FromInt(1)/2; + 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 @@ -614,22 +578,22 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, 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); + 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; vertexes.push_back(vert); + 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; vertexes.push_back(vert); + 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; vertexes.push_back(vert); + 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; @@ -645,13 +609,13 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, 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 }); + m_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 }); + 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, @@ -663,103 +627,32 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, 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); + 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 < edgeSquares.size(); ++i) + for (size_t i = 0; i < m_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) + 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 < 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; + 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(vertexes.size() < 65536); // we store array indexes as u16 + ENSURE(m_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); - } - } + DebugRenderGraph(cmpObstructionManager->GetSimContext(), m_Vertexes, m_Edges, m_EdgeSquares); // Do an A* search over the vertex/visibility graph: @@ -772,8 +665,6 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // 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); @@ -785,7 +676,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, { // Move best tile from open to closed VertexPriorityQueue::Item curr = open.pop(); - vertexes[curr.id].status = Vertex::CLOSED; + m_Vertexes[curr.id].status = Vertex::CLOSED; // If we've reached the destination, stop if (curr.id == GOAL_VERTEX_ID) @@ -798,27 +689,27 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // 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)); + if (m_EdgeSquares.size() > 8) + std::partial_sort(m_EdgeSquares.begin(), m_EdgeSquares.begin() + 8, m_EdgeSquares.end(), SquareSort(m_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); + 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 < vertexes.size(); ++n) + for (size_t n = 0; n < m_Vertexes.size(); ++n) { - if (vertexes[n].status == Vertex::CLOSED) + 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 = goal.NearestPointOnGoal(vertexes[curr.id].p); + 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 @@ -827,17 +718,17 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, npos.Y = clamp(npos.Y, rangeZMin, rangeZMax); } else - npos = vertexes[n].p; + npos = m_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; + 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 (!(vertexes[curr.id].quadOutward & quad)) + 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 @@ -846,86 +737,227 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, } 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); + 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 - m_DebugOverlayShortPathLines.push_back(SOverlayLine()); - m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); - std::vector xz; - xz.push_back(vertexes[curr.id].p.X.ToFloat()); - xz.push_back(vertexes[curr.id].p.Y.ToFloat()); - xz.push_back(npos.X.ToFloat()); - xz.push_back(npos.Y.ToFloat()); - SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false); - */ + // Render the edges that we examine. + DebugRenderEdges(cmpObstructionManager->GetSimContext(), visible, m_Vertexes[curr.id].p, npos); if (visible) { - fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length(); + 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 (vertexes[n].status == Vertex::UNEXPLORED) + if (m_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; + 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 (vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8))) - vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF; + 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) - vertexes[n].p = npos; // remember the new best goal position + m_Vertexes[n].p = npos; // remember the new best goal position - VertexPriorityQueue::Item t = { (u16)n, g + vertexes[n].h, vertexes[n].h }; + 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 (vertexes[n].h < hBest) + if (m_Vertexes[n].h < hBest) { idBest = (u16)n; - hBest = vertexes[n].h; + 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 >= vertexes[n].g) + 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 = vertexes[n].g; - vertexes[n].g = g; - vertexes[n].pred = curr.id; + 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 (vertexes[n].quadInward) - vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF; + if (m_Vertexes[n].quadInward) + m_Vertexes[n].quadOutward = ((m_Vertexes[n].quadInward) & quad) & 0xF; if (n == GOAL_VERTEX_ID) - vertexes[n].p = npos; // remember the new best goal position + m_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); + open.promote((u16)n, gprev + m_Vertexes[n].h, g + m_Vertexes[n].h, m_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 }); + 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 }); - PROFILE_END("Short pathfinding - A*"); + + 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]); } diff --git a/source/simulation2/helpers/VertexPathfinder.h b/source/simulation2/helpers/VertexPathfinder.h new file mode 100755 index 0000000000..a185d6332a --- /dev/null +++ b/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