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); | ||||
StanUnsubmitted 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 | |||||
wraitiiAuthorUnsubmitted 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… | |||||
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]); | |||||
} | } |
Wildfire Games · Phabricator
Couldn't we pass an object/ struct here instead ?