Norse_Harold/0ad
1
0
Fork 0
forked from 0ad/0ad
Code Pull Requests Activity

Optimise vertex pathfinder (typically ~6x faster).

Browse Source 
This was SVN commit r8056.
This commit is contained in:
Ykkrosh 2010-09-03 09:44:41 +00:00
parent 188a3cab12
commit b712efac07
1 changed files with 382 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

429
source/simulation2/components/CCmpPathfinder_Vertex.cpp
View File

@ -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())
return false;
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);
@ -99,7 +295,7 @@ static CFixedVector2D NearestPointOnGoal(CFixedVector2D pos, const CCmpPathfinde
return g;
}
case CCmpPathfinder::Goal::CIRCLE:
case CCmpPathfinder::Goal::CIRCLE:
{
CFixedVector2D d = pos - g;
if (d.IsZero())
@ -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,14 +610,22 @@ 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));
Edge e0 = { ev0, ev1 };
Edge e1 = { ev1, ev2 };
Edge e2 = { ev2, ev3 };
Edge e3 = { ev3, ev0 };
edges.push_back(e0);
edges.push_back(e1);
edges.push_back(e2);
edges.push_back(e3);
if (aa)
{
Edge e = { ev1, ev3 };
edgesAA.push_back(e);
}
else
{
Edge e0 = { ev0, ev1 };
Edge e1 = { ev1, ev2 };
Edge e2 = { ev2, ev3 };
Edge e3 = { ev3, ev0 };
edges.push_back(e0);
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*");
}