forked from 0ad/0ad
Optimise vertex pathfinder (typically ~6x faster).
This was SVN commit r8056.
This commit is contained in:
parent
188a3cab12
commit
b712efac07
@ -18,17 +18,71 @@
|
||||
/**
|
||||
* @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;
|
||||
|
||||
// A vertex around the corners of an obstruction
|
||||
// (paths will be sequences of these vertexes)
|
||||
struct Vertex
|
||||
{
|
||||
enum
|
||||
@ -42,13 +96,28 @@ struct Vertex
|
||||
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).
|
||||
// When used in the 'edges' list, defines the two points of the edge.
|
||||
// When used in the 'edgesAA' list, defines the opposing corners of an axis-aligned square
|
||||
// (from which four individual edges can be trivially computed), requiring p0 <= p1
|
||||
struct Edge
|
||||
{
|
||||
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;
|
||||
};
|
||||
|
||||
// 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)
|
||||
@ -58,36 +127,163 @@ static const entity_pos_t EDGE_EXPAND_DELTA = entity_pos_t::FromInt(1)/4;
|
||||
* 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.)
|
||||
*/
|
||||
static bool CheckVisibility(CFixedVector2D a, CFixedVector2D b, const std::vector<Edge>& edges)
|
||||
inline static bool CheckVisibility(CFixedVector2D a, CFixedVector2D b, const std::vector<Edge>& edges)
|
||||
{
|
||||
CFixedVector2D abn = (b - a).Perpendicular();
|
||||
|
||||
// Edges of general non-axis-aligned shapes
|
||||
for (size_t i = 0; i < edges.size(); ++i)
|
||||
{
|
||||
CFixedVector2D d = (edges[i].p1 - edges[i].p0).Perpendicular();
|
||||
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 - edges[i].p0).Dot(d);
|
||||
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 - edges[i].p0).Dot(d);
|
||||
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 = (edges[i].p0 - a).Dot(abn);
|
||||
fixed t = (edges[i].p1 - a).Dot(abn);
|
||||
if (s <= fixed::Zero() && t >= fixed::Zero())
|
||||
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(CFixedVector2D a, CFixedVector2D b, const std::vector<EdgeAA>& 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(CFixedVector2D a, CFixedVector2D b, const std::vector<EdgeAA>& 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(CFixedVector2D a, CFixedVector2D b, const std::vector<EdgeAA>& 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(CFixedVector2D a, CFixedVector2D b, const std::vector<EdgeAA>& 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;
|
||||
}
|
||||
|
||||
|
||||
static CFixedVector2D NearestPointOnGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal)
|
||||
{
|
||||
CFixedVector2D g(goal.x, goal.z);
|
||||
@ -129,7 +325,7 @@ struct TileEdge
|
||||
enum { TOP, BOTTOM, LEFT, RIGHT } dir;
|
||||
};
|
||||
|
||||
static void AddTerrainEdges(std::vector<Edge>& edges, std::vector<Vertex>& vertexes, u16 i0, u16 j0, u16 i1, u16 j1, fixed r, u8 passClass, const Grid<TerrainTile>& terrain)
|
||||
static void AddTerrainEdges(std::vector<Edge>& edgesAA, std::vector<Vertex>& vertexes, u16 i0, u16 j0, u16 i1, u16 j1, fixed r, u8 passClass, const Grid<TerrainTile>& terrain)
|
||||
{
|
||||
PROFILE("AddTerrainEdges");
|
||||
|
||||
@ -142,28 +338,44 @@ static void AddTerrainEdges(std::vector<Edge>& edges, std::vector<Vertex>& verte
|
||||
{
|
||||
if (!IS_TERRAIN_PASSABLE(terrain.get(i, j), passClass))
|
||||
{
|
||||
bool any = false; // whether we're adding any edges of this tile
|
||||
|
||||
if (j > 0 && IS_TERRAIN_PASSABLE(terrain.get(i, j-1), passClass))
|
||||
{
|
||||
TileEdge e = { i, j, TileEdge::BOTTOM };
|
||||
tileEdges.push_back(e);
|
||||
any = true;
|
||||
}
|
||||
|
||||
if (j < terrain.m_H-1 && IS_TERRAIN_PASSABLE(terrain.get(i, j+1), passClass))
|
||||
{
|
||||
TileEdge e = { i, j, TileEdge::TOP };
|
||||
tileEdges.push_back(e);
|
||||
any = true;
|
||||
}
|
||||
|
||||
if (i > 0 && IS_TERRAIN_PASSABLE(terrain.get(i-1, j), passClass))
|
||||
{
|
||||
TileEdge e = { i, j, TileEdge::LEFT };
|
||||
tileEdges.push_back(e);
|
||||
any = true;
|
||||
}
|
||||
|
||||
if (i < terrain.m_W-1 && IS_TERRAIN_PASSABLE(terrain.get(i+1, j), passClass))
|
||||
{
|
||||
TileEdge e = { i, j, TileEdge::RIGHT };
|
||||
tileEdges.push_back(e);
|
||||
any = true;
|
||||
}
|
||||
|
||||
// If we want to add any edge, then add the whole square to the axis-aligned-edges list.
|
||||
// (The inner edges are redundant but it's easier than trying to split the squares apart.)
|
||||
if (any)
|
||||
{
|
||||
CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i * CELL_SIZE) - r, fixed::FromInt(j * CELL_SIZE) - r);
|
||||
CFixedVector2D v1 = CFixedVector2D(fixed::FromInt((i+1) * CELL_SIZE) + r, fixed::FromInt((j+1) * CELL_SIZE) + r);
|
||||
Edge e = { v0, v1 };
|
||||
edgesAA.push_back(e);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -173,7 +385,7 @@ static void AddTerrainEdges(std::vector<Edge>& edges, std::vector<Vertex>& verte
|
||||
|
||||
// TODO: for efficiency (minimising the A* search space), we should coalesce adjoining edges
|
||||
|
||||
// Add all the tile edges to the search edge/vertex lists
|
||||
// Add all the tile outer edges to the search vertex lists
|
||||
for (size_t n = 0; n < tileEdges.size(); ++n)
|
||||
{
|
||||
u16 i = tileEdges[n].i;
|
||||
@ -181,6 +393,7 @@ static void AddTerrainEdges(std::vector<Edge>& edges, std::vector<Vertex>& verte
|
||||
CFixedVector2D v0, v1;
|
||||
Vertex vert;
|
||||
vert.status = Vertex::UNEXPLORED;
|
||||
vert.quadOutward = QUADRANT_ALL;
|
||||
|
||||
switch (tileEdges[n].dir)
|
||||
{
|
||||
@ -188,51 +401,94 @@ static void AddTerrainEdges(std::vector<Edge>& edges, std::vector<Vertex>& verte
|
||||
{
|
||||
v0 = CFixedVector2D(fixed::FromInt(i * CELL_SIZE) - r, fixed::FromInt(j * CELL_SIZE) - r);
|
||||
v1 = CFixedVector2D(fixed::FromInt((i+1) * CELL_SIZE) + r, fixed::FromInt(j * CELL_SIZE) - r);
|
||||
Edge e = { v0, v1 };
|
||||
edges.push_back(e);
|
||||
vert.p.X = v0.X - EDGE_EXPAND_DELTA; vert.p.Y = v0.Y - EDGE_EXPAND_DELTA; vertexes.push_back(vert);
|
||||
vert.p.X = v1.X + EDGE_EXPAND_DELTA; vert.p.Y = v1.Y - EDGE_EXPAND_DELTA; vertexes.push_back(vert);
|
||||
vert.p.X = v0.X - EDGE_EXPAND_DELTA; vert.p.Y = v0.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TR; vertexes.push_back(vert);
|
||||
vert.p.X = v1.X + EDGE_EXPAND_DELTA; vert.p.Y = v1.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TL; vertexes.push_back(vert);
|
||||
break;
|
||||
}
|
||||
case TileEdge::TOP:
|
||||
{
|
||||
v0 = CFixedVector2D(fixed::FromInt((i+1) * CELL_SIZE) + r, fixed::FromInt((j+1) * CELL_SIZE) + r);
|
||||
v1 = CFixedVector2D(fixed::FromInt(i * CELL_SIZE) - r, fixed::FromInt((j+1) * CELL_SIZE) + r);
|
||||
Edge e = { v0, v1 };
|
||||
edges.push_back(e);
|
||||
vert.p.X = v0.X + EDGE_EXPAND_DELTA; vert.p.Y = v0.Y + EDGE_EXPAND_DELTA; vertexes.push_back(vert);
|
||||
vert.p.X = v1.X - EDGE_EXPAND_DELTA; vert.p.Y = v1.Y + EDGE_EXPAND_DELTA; vertexes.push_back(vert);
|
||||
vert.p.X = v0.X + EDGE_EXPAND_DELTA; vert.p.Y = v0.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BL; vertexes.push_back(vert);
|
||||
vert.p.X = v1.X - EDGE_EXPAND_DELTA; vert.p.Y = v1.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BR; vertexes.push_back(vert);
|
||||
break;
|
||||
}
|
||||
case TileEdge::LEFT:
|
||||
{
|
||||
v0 = CFixedVector2D(fixed::FromInt(i * CELL_SIZE) - r, fixed::FromInt((j+1) * CELL_SIZE) + r);
|
||||
v1 = CFixedVector2D(fixed::FromInt(i * CELL_SIZE) - r, fixed::FromInt(j * CELL_SIZE) - r);
|
||||
Edge e = { v0, v1 };
|
||||
edges.push_back(e);
|
||||
vert.p.X = v0.X - EDGE_EXPAND_DELTA; vert.p.Y = v0.Y + EDGE_EXPAND_DELTA; vertexes.push_back(vert);
|
||||
vert.p.X = v1.X - EDGE_EXPAND_DELTA; vert.p.Y = v1.Y - EDGE_EXPAND_DELTA; vertexes.push_back(vert);
|
||||
vert.p.X = v0.X - EDGE_EXPAND_DELTA; vert.p.Y = v0.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BR; vertexes.push_back(vert);
|
||||
vert.p.X = v1.X - EDGE_EXPAND_DELTA; vert.p.Y = v1.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TR; vertexes.push_back(vert);
|
||||
break;
|
||||
}
|
||||
case TileEdge::RIGHT:
|
||||
{
|
||||
v0 = CFixedVector2D(fixed::FromInt((i+1) * CELL_SIZE) + r, fixed::FromInt(j * CELL_SIZE) - r);
|
||||
v1 = CFixedVector2D(fixed::FromInt((i+1) * CELL_SIZE) + r, fixed::FromInt((j+1) * CELL_SIZE) + r);
|
||||
Edge e = { v0, v1 };
|
||||
edges.push_back(e);
|
||||
vert.p.X = v0.X + EDGE_EXPAND_DELTA; vert.p.Y = v0.Y - EDGE_EXPAND_DELTA; vertexes.push_back(vert);
|
||||
vert.p.X = v1.X + EDGE_EXPAND_DELTA; vert.p.Y = v1.Y + EDGE_EXPAND_DELTA; vertexes.push_back(vert);
|
||||
vert.p.X = v0.X + EDGE_EXPAND_DELTA; vert.p.Y = v0.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TL; vertexes.push_back(vert);
|
||||
vert.p.X = v1.X + EDGE_EXPAND_DELTA; vert.p.Y = v1.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BL; vertexes.push_back(vert);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void SplitAAEdges(CFixedVector2D a,
|
||||
const std::vector<Edge>& edgesAA,
|
||||
std::vector<EdgeAA>& edgesLeft, std::vector<EdgeAA>& edgesRight,
|
||||
std::vector<EdgeAA>& edgesBottom, std::vector<EdgeAA>& edgesTop)
|
||||
{
|
||||
edgesLeft.reserve(edgesAA.size());
|
||||
edgesRight.reserve(edgesAA.size());
|
||||
edgesBottom.reserve(edgesAA.size());
|
||||
edgesTop.reserve(edgesAA.size());
|
||||
|
||||
for (size_t i = 0; i < edgesAA.size(); ++i)
|
||||
{
|
||||
if (a.X <= edgesAA[i].p0.X)
|
||||
{
|
||||
EdgeAA e = { edgesAA[i].p0, edgesAA[i].p1.Y };
|
||||
edgesLeft.push_back(e);
|
||||
}
|
||||
if (a.X >= edgesAA[i].p1.X)
|
||||
{
|
||||
EdgeAA e = { edgesAA[i].p1, edgesAA[i].p0.Y };
|
||||
edgesRight.push_back(e);
|
||||
}
|
||||
if (a.Y <= edgesAA[i].p0.Y)
|
||||
{
|
||||
EdgeAA e = { edgesAA[i].p0, edgesAA[i].p1.X };
|
||||
edgesBottom.push_back(e);
|
||||
}
|
||||
if (a.Y >= edgesAA[i].p1.Y)
|
||||
{
|
||||
EdgeAA e = { edgesAA[i].p1, edgesAA[i].p0.X };
|
||||
edgesTop.push_back(e);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Functor for sorting edges by approximate proximity to a fixed point.
|
||||
*/
|
||||
struct EdgeSort
|
||||
{
|
||||
CFixedVector2D src;
|
||||
EdgeSort(CFixedVector2D src) : src(src) { }
|
||||
bool operator()(const Edge& a, const Edge& b)
|
||||
{
|
||||
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 r, entity_pos_t range, const Goal& goal, u8 passClass, Path& path)
|
||||
{
|
||||
UpdateGrid(); // TODO: only need to bother updating if the terrain changed
|
||||
|
||||
PROFILE("ComputeShortPath");
|
||||
// ScopeTimer UID__(L"ComputeShortPath");
|
||||
|
||||
m_DebugOverlayShortPathLines.clear();
|
||||
|
||||
@ -265,6 +521,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
// 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<Edge> edges;
|
||||
std::vector<Edge> edgesAA; // axis-aligned squares
|
||||
|
||||
// Create impassable edges at the max-range boundary, so we can't escape the region
|
||||
// where we're meant to be searching
|
||||
@ -291,14 +548,14 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
std::vector<Vertex> vertexes;
|
||||
|
||||
// Add the start point to the graph
|
||||
Vertex start = { CFixedVector2D(x0, z0), fixed::Zero(), (CFixedVector2D(x0, z0) - goalVec).Length(), 0, Vertex::OPEN };
|
||||
Vertex start = { CFixedVector2D(x0, z0), fixed::Zero(), (CFixedVector2D(x0, z0) - goalVec).Length(), 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 };
|
||||
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;
|
||||
|
||||
@ -307,7 +564,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
u16 i0, j0, i1, j1;
|
||||
NearestTile(rangeXMin, rangeZMin, i0, j0);
|
||||
NearestTile(rangeXMax, rangeZMax, i1, j1);
|
||||
AddTerrainEdges(edges, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
|
||||
AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
|
||||
}
|
||||
|
||||
// Find all the obstruction squares that might affect us
|
||||
@ -317,7 +574,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
|
||||
// Resize arrays to reduce reallocations
|
||||
vertexes.reserve(vertexes.size() + squares.size()*4);
|
||||
edges.reserve(edges.size() + squares.size()*4);
|
||||
edgesAA.reserve(edgesAA.size() + squares.size()); // (assume most squares are AA)
|
||||
|
||||
// Convert each obstruction square into collision edges and search graph vertexes
|
||||
for (size_t i = 0; i < squares.size(); ++i)
|
||||
@ -332,14 +589,19 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
CFixedVector2D hd0(squares[i].hw + r + EDGE_EXPAND_DELTA, squares[i].hh + r + EDGE_EXPAND_DELTA);
|
||||
CFixedVector2D hd1(squares[i].hw + r + EDGE_EXPAND_DELTA, -(squares[i].hh + r + 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.p.X = center.X - hd0.Dot(u); vert.p.Y = center.Y + hd0.Dot(v); vertexes.push_back(vert);
|
||||
vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); vertexes.push_back(vert);
|
||||
vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); vertexes.push_back(vert);
|
||||
vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); vertexes.push_back(vert);
|
||||
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 - 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 + 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 + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; vertexes.push_back(vert);
|
||||
|
||||
// Add the four edges
|
||||
// Add the edges:
|
||||
|
||||
CFixedVector2D h0(squares[i].hw + r, squares[i].hh + r);
|
||||
CFixedVector2D h1(squares[i].hw + r, -(squares[i].hh + r));
|
||||
@ -348,6 +610,13 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
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)
|
||||
{
|
||||
Edge e = { ev1, ev3 };
|
||||
edgesAA.push_back(e);
|
||||
}
|
||||
else
|
||||
{
|
||||
Edge e0 = { ev0, ev1 };
|
||||
Edge e1 = { ev1, ev2 };
|
||||
Edge e2 = { ev2, ev3 };
|
||||
@ -356,6 +625,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
edges.push_back(e1);
|
||||
edges.push_back(e2);
|
||||
edges.push_back(e3);
|
||||
}
|
||||
|
||||
// TODO: should clip out vertexes and edges that are outside the range,
|
||||
// to reduce the search space
|
||||
@ -377,6 +647,24 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
xz.push_back(edges[i].p1.Y.ToFloat());
|
||||
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < edgesAA.size(); ++i)
|
||||
{
|
||||
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
|
||||
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
|
||||
std::vector<float> xz;
|
||||
xz.push_back(edgesAA[i].p0.X.ToFloat());
|
||||
xz.push_back(edgesAA[i].p0.Y.ToFloat());
|
||||
xz.push_back(edgesAA[i].p0.X.ToFloat());
|
||||
xz.push_back(edgesAA[i].p1.Y.ToFloat());
|
||||
xz.push_back(edgesAA[i].p1.X.ToFloat());
|
||||
xz.push_back(edgesAA[i].p1.Y.ToFloat());
|
||||
xz.push_back(edgesAA[i].p1.X.ToFloat());
|
||||
xz.push_back(edgesAA[i].p0.Y.ToFloat());
|
||||
xz.push_back(edgesAA[i].p0.X.ToFloat());
|
||||
xz.push_back(edgesAA[i].p0.Y.ToFloat());
|
||||
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
|
||||
}
|
||||
}
|
||||
|
||||
// Do an A* search over the vertex/visibility graph:
|
||||
@ -412,6 +700,16 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
break;
|
||||
}
|
||||
|
||||
// Sort the edges so ones nearer this vertex are checked first by CheckVisibility,
|
||||
// since they're more likely to block the rays
|
||||
std::sort(edgesAA.begin(), edgesAA.end(), EdgeSort(vertexes[curr.id].p));
|
||||
|
||||
std::vector<EdgeAA> edgesLeft;
|
||||
std::vector<EdgeAA> edgesRight;
|
||||
std::vector<EdgeAA> edgesBottom;
|
||||
std::vector<EdgeAA> edgesTop;
|
||||
SplitAAEdges(vertexes[curr.id].p, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
|
||||
|
||||
// Check the lines to every other vertex
|
||||
for (size_t n = 0; n < vertexes.size(); ++n)
|
||||
{
|
||||
@ -425,7 +723,30 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
else
|
||||
npos = vertexes[n].p;
|
||||
|
||||
bool visible = CheckVisibility(vertexes[curr.id].p, npos, edges);
|
||||
// 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, edges);
|
||||
|
||||
/*
|
||||
// Render the edges that we examine
|
||||
@ -451,8 +772,17 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
vertexes[n].g = g;
|
||||
vertexes[n].h = DistanceToGoal(npos, goal);
|
||||
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);
|
||||
|
||||
if (n == GOAL_VERTEX_ID)
|
||||
vertexes[n].p = npos; // remember the new best goal position
|
||||
|
||||
PriorityQueue::Item t = { (u16)n, g + vertexes[n].h };
|
||||
open.push(t);
|
||||
|
||||
@ -473,10 +803,16 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
// Otherwise, we have a better path, so replace the old one with the new cost/parent
|
||||
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);
|
||||
|
||||
if (n == GOAL_VERTEX_ID)
|
||||
vertexes[n].p = npos; // remember the new best goal position
|
||||
|
||||
open.promote((u16)n, g + vertexes[n].h);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -491,4 +827,3 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, enti
|
||||
|
||||
PROFILE_END("A*");
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user