Ykkrosh
05422ad545
Add CEntityHandle which wraps an entity_id_t and also has a pointer to a per-entity array of IComponents. QueryInterface/CmpPtr on a handle just involve a couple of pointer dereferences instead of a map lookup. This requires sizeof(void*) per registered interface type per entity, which is currently ~0.5KB per entity, which shouldn't be a problem. This was SVN commit r13825.
895 lines
29 KiB
C++
895 lines
29 KiB
C++
/* Copyright (C) 2012 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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
/**
|
|
* @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
|
|
{
|
|
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).
|
|
// 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)
|
|
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.)
|
|
*/
|
|
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 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 - p0).Dot(d);
|
|
if (q < fixed::Zero())
|
|
continue;
|
|
|
|
// If 'b' is in front of the edge, we can't cross
|
|
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 = (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);
|
|
|
|
switch (goal.type)
|
|
{
|
|
case CCmpPathfinder::Goal::POINT:
|
|
{
|
|
return g;
|
|
}
|
|
|
|
case CCmpPathfinder::Goal::CIRCLE:
|
|
{
|
|
CFixedVector2D d = pos - g;
|
|
if (d.IsZero())
|
|
d = CFixedVector2D(fixed::FromInt(1), fixed::Zero()); // some arbitrary direction
|
|
d.Normalize(goal.hw);
|
|
return g + d;
|
|
}
|
|
|
|
case CCmpPathfinder::Goal::SQUARE:
|
|
{
|
|
CFixedVector2D halfSize(goal.hw, goal.hh);
|
|
CFixedVector2D d = pos - g;
|
|
return g + Geometry::NearestPointOnSquare(d, goal.u, goal.v, halfSize);
|
|
}
|
|
|
|
default:
|
|
debug_warn(L"invalid type");
|
|
return CFixedVector2D();
|
|
}
|
|
}
|
|
|
|
CFixedVector2D CCmpPathfinder::GetNearestPointOnGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal)
|
|
{
|
|
return NearestPointOnGoal(pos, goal);
|
|
// (It's intentional that we don't put the implementation inside this
|
|
// function, to avoid the (admittedly unmeasured and probably trivial)
|
|
// cost of a virtual call inside ComputeShortPath)
|
|
}
|
|
|
|
typedef PriorityQueueHeap<u16, fixed> PriorityQueue;
|
|
|
|
struct TileEdge
|
|
{
|
|
u16 i, j;
|
|
enum { TOP, BOTTOM, LEFT, RIGHT } dir;
|
|
};
|
|
|
|
static void AddTerrainEdges(std::vector<Edge>& edgesAA, std::vector<Vertex>& vertexes,
|
|
u16 i0, u16 j0, u16 i1, u16 j1, fixed r,
|
|
ICmpPathfinder::pass_class_t passClass, const Grid<TerrainTile>& terrain)
|
|
{
|
|
PROFILE("AddTerrainEdges");
|
|
|
|
std::vector<TileEdge> tileEdges;
|
|
|
|
// Find all edges between tiles of differently passability statuses
|
|
for (u16 j = j0; j <= j1; ++j)
|
|
{
|
|
for (u16 i = i0; i <= i1; ++i)
|
|
{
|
|
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 * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r);
|
|
CFixedVector2D v1 = CFixedVector2D(fixed::FromInt((i+1) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r);
|
|
Edge e = { v0, v1 };
|
|
edgesAA.push_back(e);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// TODO: maybe we should precompute these terrain edges since they'll rarely change?
|
|
|
|
// TODO: for efficiency (minimising the A* search space), we should coalesce adjoining edges
|
|
|
|
// 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;
|
|
u16 j = tileEdges[n].j;
|
|
CFixedVector2D v0, v1;
|
|
Vertex vert;
|
|
vert.status = Vertex::UNEXPLORED;
|
|
vert.quadOutward = QUADRANT_ALL;
|
|
|
|
switch (tileEdges[n].dir)
|
|
{
|
|
case TileEdge::BOTTOM:
|
|
{
|
|
v0 = CFixedVector2D(fixed::FromInt(i * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r);
|
|
v1 = CFixedVector2D(fixed::FromInt((i+1) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r);
|
|
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) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r);
|
|
v1 = CFixedVector2D(fixed::FromInt(i * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r);
|
|
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 * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r);
|
|
v1 = CFixedVector2D(fixed::FromInt(i * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r);
|
|
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) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r);
|
|
v1 = CFixedVector2D(fixed::FromInt((i+1) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r);
|
|
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, pass_class_t passClass, Path& path)
|
|
{
|
|
UpdateGrid(); // TODO: only need to bother updating if the terrain changed
|
|
|
|
PROFILE3("ComputeShortPath");
|
|
// ScopeTimer UID__(L"ComputeShortPath");
|
|
|
|
m_DebugOverlayShortPathLines.clear();
|
|
|
|
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 CCmpPathfinder::Goal::POINT:
|
|
{
|
|
SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true);
|
|
break;
|
|
}
|
|
case CCmpPathfinder::Goal::CIRCLE:
|
|
{
|
|
SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true);
|
|
break;
|
|
}
|
|
case CCmpPathfinder::Goal::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.)
|
|
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
|
|
fixed rangeXMin = x0 - range;
|
|
fixed rangeXMax = x0 + range;
|
|
fixed rangeZMin = z0 - range;
|
|
fixed rangeZMax = z0 + range;
|
|
{
|
|
// (The edges are the opposite direction to usual, so it's an inside-out square)
|
|
Edge e0 = { CFixedVector2D(rangeXMin, rangeZMin), CFixedVector2D(rangeXMin, rangeZMax) };
|
|
Edge e1 = { CFixedVector2D(rangeXMin, rangeZMax), CFixedVector2D(rangeXMax, rangeZMax) };
|
|
Edge e2 = { CFixedVector2D(rangeXMax, rangeZMax), CFixedVector2D(rangeXMax, rangeZMin) };
|
|
Edge e3 = { CFixedVector2D(rangeXMax, rangeZMin), CFixedVector2D(rangeXMin, rangeZMin) };
|
|
edges.push_back(e0);
|
|
edges.push_back(e1);
|
|
edges.push_back(e2);
|
|
edges.push_back(e3);
|
|
}
|
|
|
|
// List of obstruction vertexes (plus start/end points); we'll try to find paths through
|
|
// the graph defined by these vertexes
|
|
std::vector<Vertex> vertexes;
|
|
|
|
// Add the start point to the graph
|
|
CFixedVector2D posStart(x0, z0);
|
|
fixed hStart = (posStart - NearestPointOnGoal(posStart, goal)).Length();
|
|
Vertex start = { posStart, fixed::Zero(), hStart, 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, QUADRANT_NONE, QUADRANT_ALL };
|
|
vertexes.push_back(end);
|
|
const size_t GOAL_VERTEX_ID = 1;
|
|
|
|
// Add terrain obstructions
|
|
{
|
|
u16 i0, j0, i1, j1;
|
|
NearestTile(rangeXMin, rangeZMin, i0, j0);
|
|
NearestTile(rangeXMax, rangeZMax, i1, j1);
|
|
AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
|
|
}
|
|
|
|
// Find all the obstruction squares that might affect us
|
|
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSystemEntity());
|
|
std::vector<ICmpObstructionManager::ObstructionSquare> squares;
|
|
cmpObstructionManager->GetObstructionsInRange(filter, rangeXMin - r, rangeZMin - r, rangeXMax + r, rangeZMax + r, squares);
|
|
|
|
// Resize arrays to reduce reallocations
|
|
vertexes.reserve(vertexes.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)
|
|
{
|
|
CFixedVector2D center(squares[i].x, squares[i].z);
|
|
CFixedVector2D u = squares[i].u;
|
|
CFixedVector2D v = squares[i].v;
|
|
|
|
// Expand the vertexes by the moving unit's collision radius, to find the
|
|
// closest we can get to it
|
|
|
|
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.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 edges:
|
|
|
|
CFixedVector2D h0(squares[i].hw + r, squares[i].hh + r);
|
|
CFixedVector2D h1(squares[i].hw + r, -(squares[i].hh + r));
|
|
|
|
CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v));
|
|
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 };
|
|
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
|
|
}
|
|
|
|
ENSURE(vertexes.size() < 65536); // we store array indexes as u16
|
|
|
|
if (m_DebugOverlay)
|
|
{
|
|
// Render the obstruction edges
|
|
for (size_t i = 0; i < edges.size(); ++i)
|
|
{
|
|
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
|
|
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
|
|
std::vector<float> xz;
|
|
xz.push_back(edges[i].p0.X.ToFloat());
|
|
xz.push_back(edges[i].p0.Y.ToFloat());
|
|
xz.push_back(edges[i].p1.X.ToFloat());
|
|
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:
|
|
|
|
// 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.
|
|
|
|
// 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
|
|
// vertex and see if we can reach it without hitting any collision edges, and ignore the ones
|
|
// 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("A*");
|
|
|
|
PriorityQueue open;
|
|
PriorityQueue::Item qiStart = { START_VERTEX_ID, start.h };
|
|
open.push(qiStart);
|
|
|
|
u16 idBest = START_VERTEX_ID;
|
|
fixed hBest = start.h;
|
|
|
|
while (!open.empty())
|
|
{
|
|
// Move best tile from open to closed
|
|
PriorityQueue::Item curr = open.pop();
|
|
vertexes[curr.id].status = Vertex::CLOSED;
|
|
|
|
// If we've reached the destination, stop
|
|
if (curr.id == GOAL_VERTEX_ID)
|
|
{
|
|
idBest = curr.id;
|
|
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)
|
|
{
|
|
if (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 = NearestPointOnGoal(vertexes[curr.id].p, goal);
|
|
|
|
// 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
|
|
// sometimes it happens), so clamp it to the current search range
|
|
npos.X = clamp(npos.X, rangeXMin, rangeXMax);
|
|
npos.Y = clamp(npos.Y, rangeZMin, rangeZMax);
|
|
}
|
|
else
|
|
{
|
|
npos = 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;
|
|
|
|
// 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
|
|
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<float> 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);
|
|
//*/
|
|
|
|
if (visible)
|
|
{
|
|
fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length();
|
|
|
|
// If this is a new tile, compute the heuristic distance
|
|
if (vertexes[n].status == Vertex::UNEXPLORED)
|
|
{
|
|
// Add it to the open list:
|
|
vertexes[n].status = Vertex::OPEN;
|
|
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) & 0xF;
|
|
|
|
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);
|
|
|
|
// Remember the heuristically best vertex we've seen so far, in case we never actually reach the target
|
|
if (vertexes[n].h < hBest)
|
|
{
|
|
idBest = (u16)n;
|
|
hBest = 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)
|
|
continue;
|
|
|
|
// 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) & 0xF;
|
|
|
|
if (n == GOAL_VERTEX_ID)
|
|
vertexes[n].p = npos; // remember the new best goal position
|
|
|
|
open.promote((u16)n, g + vertexes[n].h);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Reconstruct the path (in reverse)
|
|
for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred)
|
|
{
|
|
Waypoint w = { vertexes[id].p.X, vertexes[id].p.Y };
|
|
path.m_Waypoints.push_back(w);
|
|
}
|
|
|
|
PROFILE_END("A*");
|
|
}
|
|
|
|
bool CCmpPathfinder::CheckMovement(const IObstructionTestFilter& filter,
|
|
entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r,
|
|
pass_class_t passClass)
|
|
{
|
|
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSystemEntity());
|
|
if (!cmpObstructionManager)
|
|
return false;
|
|
|
|
if (cmpObstructionManager->TestLine(filter, x0, z0, x1, z1, r))
|
|
return false;
|
|
|
|
// Test against terrain:
|
|
|
|
// (TODO: this could probably be a tiny bit faster by not reusing all the vertex computation code)
|
|
|
|
UpdateGrid();
|
|
|
|
std::vector<Edge> edgesAA;
|
|
std::vector<Vertex> vertexes;
|
|
|
|
u16 i0, j0, i1, j1;
|
|
NearestTile(std::min(x0, x1) - r, std::min(z0, z1) - r, i0, j0);
|
|
NearestTile(std::max(x0, x1) + r, std::max(z0, z1) + r, i1, j1);
|
|
AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
|
|
|
|
CFixedVector2D a(x0, z0);
|
|
CFixedVector2D b(x1, z1);
|
|
|
|
std::vector<EdgeAA> edgesLeft;
|
|
std::vector<EdgeAA> edgesRight;
|
|
std::vector<EdgeAA> edgesBottom;
|
|
std::vector<EdgeAA> edgesTop;
|
|
SplitAAEdges(a, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
|
|
|
|
bool visible =
|
|
CheckVisibilityLeft(a, b, edgesLeft) &&
|
|
CheckVisibilityRight(a, b, edgesRight) &&
|
|
CheckVisibilityBottom(a, b, edgesBottom) &&
|
|
CheckVisibilityTop(a, b, edgesTop);
|
|
|
|
return visible;
|
|
}
|