Changeset View
Changeset View
Standalone View
Standalone View
source/simulation2/helpers/VertexPathfinder.cpp
/* Copyright (C) 2019 Wildfire Games. | /* Copyright (C) 2021 Wildfire Games. | ||||
* This file is part of 0 A.D. | * This file is part of 0 A.D. | ||||
* | * | ||||
* 0 A.D. is free software: you can redistribute it and/or modify | * 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 | * it under the terms of the GNU General Public License as published by | ||||
* the Free Software Foundation, either version 2 of the License, or | * the Free Software Foundation, either version 2 of the License, or | ||||
* (at your option) any later version. | * (at your option) any later version. | ||||
* | * | ||||
* 0 A.D. is distributed in the hope that it will be useful, | * 0 A.D. is distributed in the hope that it will be useful, | ||||
Show All 28 Lines | |||||
#include "ps/Profile.h" | #include "ps/Profile.h" | ||||
#include "renderer/Scene.h" | #include "renderer/Scene.h" | ||||
#include "simulation2/components/ICmpObstructionManager.h" | #include "simulation2/components/ICmpObstructionManager.h" | ||||
#include "simulation2/helpers/Grid.h" | #include "simulation2/helpers/Grid.h" | ||||
#include "simulation2/helpers/PriorityQueue.h" | #include "simulation2/helpers/PriorityQueue.h" | ||||
#include "simulation2/helpers/Render.h" | #include "simulation2/helpers/Render.h" | ||||
#include "simulation2/system/SimContext.h" | #include "simulation2/system/SimContext.h" | ||||
#include <mutex> | |||||
namespace | |||||
{ | |||||
static std::mutex g_DebugMutex; | |||||
} | |||||
VertexPathfinderDebugOverlay g_VertexPathfinderDebugOverlay; | |||||
/* Quadrant optimisation: | /* Quadrant optimisation: | ||||
* (loosely based on GPG2 "Optimizing Points-of-Visibility Pathfinding") | * (loosely based on GPG2 "Optimizing Points-of-Visibility Pathfinding") | ||||
* | * | ||||
* Consider the vertex ("@") at a corner of an axis-aligned rectangle ("#"): | * Consider the vertex ("@") at a corner of an axis-aligned rectangle ("#"): | ||||
* | * | ||||
* TL : TR | * TL : TR | ||||
* : | * : | ||||
* ####@ - - - | * ####@ - - - | ||||
▲ Show 20 Lines • Show All 385 Lines • ▼ Show 20 Lines | static void AddTerrainEdges(std::vector<Edge>& edges, std::vector<Vertex>& vertexes, | ||||
} | } | ||||
} | } | ||||
static void SplitAAEdges(const CFixedVector2D& a, | static void SplitAAEdges(const CFixedVector2D& a, | ||||
const std::vector<Edge>& edges, | const std::vector<Edge>& edges, | ||||
const std::vector<Square>& squares, | const std::vector<Square>& squares, | ||||
std::vector<Edge>& edgesUnaligned, | std::vector<Edge>& edgesUnaligned, | ||||
std::vector<EdgeAA>& edgesLeft, std::vector<EdgeAA>& edgesRight, | std::vector<EdgeAA>& edgesLeft, std::vector<EdgeAA>& edgesRight, | ||||
std::vector<EdgeAA>& edgesBottom, std::vector<EdgeAA>& edgesTop) | std::vector<EdgeAA>& edgesBottom, std::vector<EdgeAA>& edgesTop) | ||||
Stan: Couldn't we pass an object/ struct here instead ? | |||||
{ | { | ||||
for (const Square& square : squares) | for (const Square& square : squares) | ||||
{ | { | ||||
if (a.X <= square.p0.X) | if (a.X <= square.p0.X) | ||||
edgesLeft.emplace_back(EdgeAA{ square.p0, square.p1.Y }); | edgesLeft.emplace_back(EdgeAA{ square.p0, square.p1.Y }); | ||||
if (a.X >= square.p1.X) | if (a.X >= square.p1.X) | ||||
edgesRight.emplace_back(EdgeAA{ square.p1, square.p0.Y }); | edgesRight.emplace_back(EdgeAA{ square.p1, square.p0.Y }); | ||||
▲ Show 20 Lines • Show All 54 Lines • ▼ Show 20 Lines | bool operator()(const Square& a, const Square& b) const | ||||
return false; | return false; | ||||
} | } | ||||
}; | }; | ||||
WaypointPath VertexPathfinder::ComputeShortPath(const ShortPathRequest& request, CmpPtr<ICmpObstructionManager> cmpObstructionManager) const | WaypointPath VertexPathfinder::ComputeShortPath(const ShortPathRequest& request, CmpPtr<ICmpObstructionManager> cmpObstructionManager) const | ||||
{ | { | ||||
PROFILE2("ComputeShortPath"); | PROFILE2("ComputeShortPath"); | ||||
DebugRenderGoal(cmpObstructionManager->GetSimContext(), request.goal); | g_VertexPathfinderDebugOverlay.DebugRenderGoal(cmpObstructionManager->GetSimContext(), request.goal); | ||||
// Create impassable edges at the max-range boundary, so we can't escape the region | // Create impassable edges at the max-range boundary, so we can't escape the region | ||||
// where we're meant to be searching | // where we're meant to be searching | ||||
fixed rangeXMin = request.x0 - request.range; | fixed rangeXMin = request.x0 - request.range; | ||||
fixed rangeXMax = request.x0 + request.range; | fixed rangeXMax = request.x0 + request.range; | ||||
fixed rangeZMin = request.z0 - request.range; | fixed rangeZMin = request.z0 - request.range; | ||||
fixed rangeZMax = request.z0 + request.range; | fixed rangeZMax = request.z0 + request.range; | ||||
▲ Show 20 Lines • Show All 69 Lines • ▼ Show 20 Lines | for (size_t i = 0; i < squares.size(); ++i) | ||||
CFixedVector2D hd1(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 | // 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)); | bool aa = (u.X == fixed::FromInt(1) && u.Y == fixed::Zero() && v.X == fixed::Zero() && v.Y == fixed::FromInt(1)); | ||||
Vertex vert; | Vertex vert; | ||||
vert.status = Vertex::UNEXPLORED; | vert.status = Vertex::UNEXPLORED; | ||||
vert.quadInward = QUADRANT_NONE; | vert.quadInward = QUADRANT_NONE; | ||||
vert.quadOutward = QUADRANT_ALL; | vert.quadOutward = QUADRANT_ALL; | ||||
Not Done Inline Actions@vladislavbelov Good usage for clamp below, no ? Stan: @vladislavbelov Good usage for clamp below, no ? | |||||
vert.p.X = center.X - hd0.Dot(u); | vert.p.X = center.X - hd0.Dot(u); | ||||
vert.p.Y = center.Y + hd0.Dot(v); | vert.p.Y = center.Y + hd0.Dot(v); | ||||
if (aa) | if (aa) | ||||
{ | { | ||||
vert.quadInward = QUADRANT_BR; | vert.quadInward = QUADRANT_BR; | ||||
vert.quadOutward = (~vert.quadInward) & 0xF; | vert.quadOutward = (~vert.quadInward) & 0xF; | ||||
} | } | ||||
if (vert.p.X >= rangeXMin && vert.p.Y >= rangeZMin && vert.p.X <= rangeXMax && vert.p.Y <= rangeZMax) | if (vert.p.X >= rangeXMin && vert.p.Y >= rangeZMin && vert.p.X <= rangeXMax && vert.p.Y <= rangeZMax) | ||||
▲ Show 20 Lines • Show All 75 Lines • ▼ Show 20 Lines | for (size_t j = 2; j < m_Vertexes.size(); ++j) | ||||
m_Vertexes[j].p.Y >= m_EdgeSquares[i].p0.Y && | m_Vertexes[j].p.Y >= m_EdgeSquares[i].p0.Y && | ||||
m_Vertexes[j].p.X <= m_EdgeSquares[i].p1.X && | m_Vertexes[j].p.X <= m_EdgeSquares[i].p1.X && | ||||
m_Vertexes[j].p.Y <= m_EdgeSquares[i].p1.Y) | m_Vertexes[j].p.Y <= m_EdgeSquares[i].p1.Y) | ||||
m_Vertexes[j].status = Vertex::CLOSED; | m_Vertexes[j].status = Vertex::CLOSED; | ||||
} | } | ||||
ENSURE(m_Vertexes.size() < 65536); // We store array indexes as u16. | ENSURE(m_Vertexes.size() < 65536); // We store array indexes as u16. | ||||
DebugRenderGraph(cmpObstructionManager->GetSimContext(), m_Vertexes, m_Edges, m_EdgeSquares); | g_VertexPathfinderDebugOverlay.DebugRenderGraph(cmpObstructionManager->GetSimContext(), m_Vertexes, m_Edges, m_EdgeSquares); | ||||
// Do an A* search over the vertex/visibility graph: | // Do an A* search over the vertex/visibility graph: | ||||
// Since we are just measuring Euclidean distance the heuristic is admissible, | // 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. | // 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; | // 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 | // we do it lazily instead. When the search algorithm reaches a vertex, we examine every other | ||||
▲ Show 20 Lines • Show All 75 Lines • ▼ Show 20 Lines | for (size_t n = 0; n < m_Vertexes.size(); ++n) | ||||
bool visible = | bool visible = | ||||
CheckVisibilityLeft(m_Vertexes[curr.id].p, npos, m_EdgesLeft) && | CheckVisibilityLeft(m_Vertexes[curr.id].p, npos, m_EdgesLeft) && | ||||
CheckVisibilityRight(m_Vertexes[curr.id].p, npos, m_EdgesRight) && | CheckVisibilityRight(m_Vertexes[curr.id].p, npos, m_EdgesRight) && | ||||
CheckVisibilityBottom(m_Vertexes[curr.id].p, npos, m_EdgesBottom) && | CheckVisibilityBottom(m_Vertexes[curr.id].p, npos, m_EdgesBottom) && | ||||
CheckVisibilityTop(m_Vertexes[curr.id].p, npos, m_EdgesTop) && | CheckVisibilityTop(m_Vertexes[curr.id].p, npos, m_EdgesTop) && | ||||
CheckVisibility(m_Vertexes[curr.id].p, npos, m_EdgesUnaligned); | CheckVisibility(m_Vertexes[curr.id].p, npos, m_EdgesUnaligned); | ||||
// Render the edges that we examine. | // Render the edges that we examine. | ||||
DebugRenderEdges(cmpObstructionManager->GetSimContext(), visible, m_Vertexes[curr.id].p, npos); | g_VertexPathfinderDebugOverlay.DebugRenderEdges(cmpObstructionManager->GetSimContext(), visible, m_Vertexes[curr.id].p, npos); | ||||
if (visible) | if (visible) | ||||
{ | { | ||||
fixed g = m_Vertexes[curr.id].g + (m_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 this is a new tile, compute the heuristic distance | ||||
if (m_Vertexes[n].status == Vertex::UNEXPLORED) | if (m_Vertexes[n].status == Vertex::UNEXPLORED) | ||||
{ | { | ||||
▲ Show 20 Lines • Show All 51 Lines • ▼ Show 20 Lines | WaypointPath VertexPathfinder::ComputeShortPath(const ShortPathRequest& request, CmpPtr<ICmpObstructionManager> cmpObstructionManager) const | ||||
m_EdgesLeft.clear(); | m_EdgesLeft.clear(); | ||||
m_EdgesRight.clear(); | m_EdgesRight.clear(); | ||||
m_EdgesBottom.clear(); | m_EdgesBottom.clear(); | ||||
m_EdgesTop.clear(); | m_EdgesTop.clear(); | ||||
return path; | return path; | ||||
} | } | ||||
void VertexPathfinder::DebugRenderGoal(const CSimContext& simContext, const PathGoal& goal) const | void VertexPathfinderDebugOverlay::DebugRenderGoal(const CSimContext& simContext, const PathGoal& goal) | ||||
{ | { | ||||
if (!m_DebugOverlay) | if (!m_DebugOverlay) | ||||
return; | return; | ||||
m_DebugOverlayShortPathLines.clear(); | std::lock_guard<std::mutex> lock(g_DebugMutex); | ||||
g_VertexPathfinderDebugOverlay.m_DebugOverlayShortPathLines.clear(); | |||||
// Render the goal shape | // Render the goal shape | ||||
m_DebugOverlayShortPathLines.push_back(SOverlayLine()); | m_DebugOverlayShortPathLines.push_back(SOverlayLine()); | ||||
m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1); | m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1); | ||||
switch (goal.type) | switch (goal.type) | ||||
{ | { | ||||
case PathGoal::POINT: | case PathGoal::POINT: | ||||
{ | { | ||||
Show All 11 Lines | switch (goal.type) | ||||
{ | { | ||||
float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat()); | 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); | SimRender::ConstructSquareOnGround(simContext, goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true); | ||||
break; | break; | ||||
} | } | ||||
} | } | ||||
} | } | ||||
void VertexPathfinder::DebugRenderGraph(const CSimContext& simContext, const std::vector<Vertex>& vertexes, const std::vector<Edge>& edges, const std::vector<Square>& edgeSquares) const | void VertexPathfinderDebugOverlay::DebugRenderGraph(const CSimContext& simContext, const std::vector<Vertex>& vertexes, const std::vector<Edge>& edges, const std::vector<Square>& edgeSquares) | ||||
{ | { | ||||
if (!m_DebugOverlay) | if (!m_DebugOverlay) | ||||
return; | return; | ||||
std::lock_guard<std::mutex> lock(g_DebugMutex); | |||||
#define PUSH_POINT(p) STMT(xz.push_back(p.X.ToFloat()); xz.push_back(p.Y.ToFloat())) | #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 | // Render the vertexes as little Pac-Man shapes to indicate quadrant direction | ||||
for (size_t i = 0; i < vertexes.size(); ++i) | for (size_t i = 0; i < vertexes.size(); ++i) | ||||
{ | { | ||||
m_DebugOverlayShortPathLines.emplace_back(); | m_DebugOverlayShortPathLines.emplace_back(); | ||||
m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1); | m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1); | ||||
float x = vertexes[i].p.X.ToFloat(); | float x = vertexes[i].p.X.ToFloat(); | ||||
▲ Show 20 Lines • Show All 54 Lines • ▼ Show 20 Lines | for (size_t i = 0; i < edgeSquares.size(); ++i) | ||||
xz.push_back(s.p1.X.ToFloat()); | xz.push_back(s.p1.X.ToFloat()); | ||||
xz.push_back(s.p0.Y.ToFloat()); | xz.push_back(s.p0.Y.ToFloat()); | ||||
xz.push_back(s.p0.X.ToFloat()); | xz.push_back(s.p0.X.ToFloat()); | ||||
xz.push_back(s.p0.Y.ToFloat()); | xz.push_back(s.p0.Y.ToFloat()); | ||||
SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), true); | 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 | void VertexPathfinderDebugOverlay::DebugRenderEdges(const CSimContext& UNUSED(simContext), bool UNUSED(visible), CFixedVector2D UNUSED(curr), CFixedVector2D UNUSED(npos)) | ||||
{ | { | ||||
if (!m_DebugOverlay) | if (!m_DebugOverlay) | ||||
return; | return; | ||||
std::lock_guard<std::mutex> lock(g_DebugMutex); | |||||
// Disabled by default. | // Disabled by default. | ||||
/* | /* | ||||
m_DebugOverlayShortPathLines.push_back(SOverlayLine()); | 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.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); | ||||
m_DebugOverlayShortPathLines.push_back(SOverlayLine()); | 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.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); | ||||
std::vector<float> xz; | std::vector<float> xz; | ||||
xz.push_back(curr.X.ToFloat()); | xz.push_back(curr.X.ToFloat()); | ||||
xz.push_back(curr.Y.ToFloat()); | xz.push_back(curr.Y.ToFloat()); | ||||
xz.push_back(npos.X.ToFloat()); | xz.push_back(npos.X.ToFloat()); | ||||
xz.push_back(npos.Y.ToFloat()); | xz.push_back(npos.Y.ToFloat()); | ||||
SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false); | SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false); | ||||
SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false); | SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false); | ||||
*/ | */ | ||||
} | } | ||||
void VertexPathfinder::RenderSubmit(SceneCollector& collector) | void VertexPathfinderDebugOverlay::RenderSubmit(SceneCollector& collector) | ||||
{ | { | ||||
if (!m_DebugOverlay) | if (!m_DebugOverlay) | ||||
return; | return; | ||||
for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i) | std::lock_guard<std::mutex> lock(g_DebugMutex); | ||||
collector.Submit(&m_DebugOverlayShortPathLines[i]); | m_DebugOverlayShortPathLines.swap(m_DebugOverlayShortPathLinesSubmitted); | ||||
for (size_t i = 0; i < m_DebugOverlayShortPathLinesSubmitted.size(); ++i) | |||||
collector.Submit(&m_DebugOverlayShortPathLinesSubmitted[i]); | |||||
Not Done Inline ActionsDo you need to lock more than the swap? Same question for the functions above Stan: Do you need to lock more than the swap? Same question for the functions above | |||||
Done Inline ActionsYes, or the multiple vertex pathfinders will muck it up. The lock is really not that necessary for the swap. Not that it matters, it only locks if m_DebugOverlay, so it's usually trivially fast. wraitii: Yes, or the multiple vertex pathfinders will muck it up. The lock is really not that necessary… | |||||
} | } |
Wildfire Games · Phabricator
Couldn't we pass an object/ struct here instead ?