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();
}
}
/**
* Debug visualisation of graph edges between regions.
*/
void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass)
{
const EdgesMap& edges = m_Edges[passClass];
const std::vector<Chunk>& chunks = m_Chunks[passClass];
for (auto& edge : edges)
{
for (const RegionID& region: edge.second)
{
// Draw a line between the two regions' centers
int i0, j0, i1, j1;
chunks[edge.first.cj * m_ChunksW + edge.first.ci].RegionCenter(edge.first.r, i0, j0);
chunks[region.cj * m_ChunksW + region.ci].RegionCenter(region.r, i1, j1);
CFixedVector2D a, b;
Pathfinding::NavcellCenter(i0, j0, a.X, a.Y);
Pathfinding::NavcellCenter(i1, j1, b.X, b.Y);
// Push the endpoints inwards a little to avoid overlaps
CFixedVector2D d = b - a;
d.Normalize(entity_pos_t::FromInt(1));
a += d;
b -= d;
std::vector<float> xz;
xz.push_back(a.X.ToFloat());
xz.push_back(a.Y.ToFloat());
xz.push_back(b.X.ToFloat());
xz.push_back(b.Y.ToFloat());
m_DebugOverlayLines.emplace_back();
m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0);
SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);
}
}
}