Index: ps/trunk/source/simulation2/components/CCmpObstructionManager.cpp
===================================================================
--- ps/trunk/source/simulation2/components/CCmpObstructionManager.cpp (revision 22472)
+++ ps/trunk/source/simulation2/components/CCmpObstructionManager.cpp (revision 22473)
@@ -1,1347 +1,1347 @@
/* Copyright (C) 2019 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 .
*/
#include "precompiled.h"
#include "simulation2/system/Component.h"
#include "ICmpObstructionManager.h"
#include "ICmpTerrain.h"
#include "ICmpPosition.h"
#include "simulation2/MessageTypes.h"
#include "simulation2/helpers/Geometry.h"
#include "simulation2/helpers/Rasterize.h"
#include "simulation2/helpers/Render.h"
#include "simulation2/helpers/Spatial.h"
#include "simulation2/serialization/SerializeTemplates.h"
#include "graphics/Overlay.h"
#include "graphics/Terrain.h"
#include "maths/MathUtil.h"
#include "ps/Profile.h"
#include "renderer/Scene.h"
#include "ps/CLogger.h"
// Externally, tags are opaque non-zero positive integers.
// Internally, they are tagged (by shape) indexes into shape lists.
// idx must be non-zero.
#define TAG_IS_VALID(tag) ((tag).valid())
#define TAG_IS_UNIT(tag) (((tag).n & 1) == 0)
#define TAG_IS_STATIC(tag) (((tag).n & 1) == 1)
#define UNIT_INDEX_TO_TAG(idx) tag_t(((idx) << 1) | 0)
#define STATIC_INDEX_TO_TAG(idx) tag_t(((idx) << 1) | 1)
#define TAG_TO_INDEX(tag) ((tag).n >> 1)
/**
* Internal representation of axis-aligned circular shapes for moving units
*/
struct UnitShape
{
entity_id_t entity;
entity_pos_t x, z;
entity_pos_t clearance;
ICmpObstructionManager::flags_t flags;
entity_id_t group; // control group (typically the owner entity, or a formation controller entity) (units ignore collisions with others in the same group)
};
/**
* Internal representation of arbitrary-rotation static square shapes for buildings
*/
struct StaticShape
{
entity_id_t entity;
entity_pos_t x, z; // world-space coordinates
CFixedVector2D u, v; // orthogonal unit vectors - axes of local coordinate space
entity_pos_t hw, hh; // half width/height in local coordinate space
ICmpObstructionManager::flags_t flags;
entity_id_t group;
entity_id_t group2;
};
/**
* Serialization helper template for UnitShape
*/
struct SerializeUnitShape
{
template
void operator()(S& serialize, const char* UNUSED(name), UnitShape& value) const
{
serialize.NumberU32_Unbounded("entity", value.entity);
serialize.NumberFixed_Unbounded("x", value.x);
serialize.NumberFixed_Unbounded("z", value.z);
serialize.NumberFixed_Unbounded("clearance", value.clearance);
serialize.NumberU8_Unbounded("flags", value.flags);
serialize.NumberU32_Unbounded("group", value.group);
}
};
/**
* Serialization helper template for StaticShape
*/
struct SerializeStaticShape
{
template
void operator()(S& serialize, const char* UNUSED(name), StaticShape& value) const
{
serialize.NumberU32_Unbounded("entity", value.entity);
serialize.NumberFixed_Unbounded("x", value.x);
serialize.NumberFixed_Unbounded("z", value.z);
serialize.NumberFixed_Unbounded("u.x", value.u.X);
serialize.NumberFixed_Unbounded("u.y", value.u.Y);
serialize.NumberFixed_Unbounded("v.x", value.v.X);
serialize.NumberFixed_Unbounded("v.y", value.v.Y);
serialize.NumberFixed_Unbounded("hw", value.hw);
serialize.NumberFixed_Unbounded("hh", value.hh);
serialize.NumberU8_Unbounded("flags", value.flags);
serialize.NumberU32_Unbounded("group", value.group);
serialize.NumberU32_Unbounded("group2", value.group2);
}
};
class CCmpObstructionManager : public ICmpObstructionManager
{
public:
static void ClassInit(CComponentManager& componentManager)
{
componentManager.SubscribeToMessageType(MT_RenderSubmit); // for debug overlays
}
DEFAULT_COMPONENT_ALLOCATOR(ObstructionManager)
bool m_DebugOverlayEnabled;
bool m_DebugOverlayDirty;
std::vector m_DebugOverlayLines;
SpatialSubdivision m_UnitSubdivision;
SpatialSubdivision m_StaticSubdivision;
// TODO: using std::map is a bit inefficient; is there a better way to store these?
std::map m_UnitShapes;
std::map m_StaticShapes;
u32 m_UnitShapeNext; // next allocated id
u32 m_StaticShapeNext;
entity_pos_t m_MaxClearance;
bool m_PassabilityCircular;
entity_pos_t m_WorldX0;
entity_pos_t m_WorldZ0;
entity_pos_t m_WorldX1;
entity_pos_t m_WorldZ1;
u16 m_TerrainTiles;
static std::string GetSchema()
{
return "";
}
virtual void Init(const CParamNode& UNUSED(paramNode))
{
m_DebugOverlayEnabled = false;
m_DebugOverlayDirty = true;
m_UnitShapeNext = 1;
m_StaticShapeNext = 1;
m_UpdateInformations.dirty = true;
m_UpdateInformations.globallyDirty = true;
m_PassabilityCircular = false;
m_WorldX0 = m_WorldZ0 = m_WorldX1 = m_WorldZ1 = entity_pos_t::Zero();
m_TerrainTiles = 0;
// Initialise with bogus values (these will get replaced when
// SetBounds is called)
ResetSubdivisions(entity_pos_t::FromInt(1024), entity_pos_t::FromInt(1024));
}
virtual void Deinit()
{
}
template
void SerializeCommon(S& serialize)
{
SerializeSpatialSubdivision()(serialize, "unit subdiv", m_UnitSubdivision);
SerializeSpatialSubdivision()(serialize, "static subdiv", m_StaticSubdivision);
serialize.NumberFixed_Unbounded("max clearance", m_MaxClearance);
SerializeMap()(serialize, "unit shapes", m_UnitShapes);
SerializeMap()(serialize, "static shapes", m_StaticShapes);
serialize.NumberU32_Unbounded("unit shape next", m_UnitShapeNext);
serialize.NumberU32_Unbounded("static shape next", m_StaticShapeNext);
serialize.Bool("circular", m_PassabilityCircular);
serialize.NumberFixed_Unbounded("world x0", m_WorldX0);
serialize.NumberFixed_Unbounded("world z0", m_WorldZ0);
serialize.NumberFixed_Unbounded("world x1", m_WorldX1);
serialize.NumberFixed_Unbounded("world z1", m_WorldZ1);
serialize.NumberU16_Unbounded("terrain tiles", m_TerrainTiles);
}
virtual void Serialize(ISerializer& serialize)
{
// TODO: this could perhaps be optimised by not storing all the obstructions,
// and instead regenerating them from the other entities on Deserialize
SerializeCommon(serialize);
}
virtual void Deserialize(const CParamNode& paramNode, IDeserializer& deserialize)
{
Init(paramNode);
SerializeCommon(deserialize);
m_UpdateInformations.dirtinessGrid = Grid(m_TerrainTiles*Pathfinding::NAVCELLS_PER_TILE, m_TerrainTiles*Pathfinding::NAVCELLS_PER_TILE);
}
virtual void HandleMessage(const CMessage& msg, bool UNUSED(global))
{
switch (msg.GetType())
{
case MT_RenderSubmit:
{
const CMessageRenderSubmit& msgData = static_cast (msg);
RenderSubmit(msgData.collector);
break;
}
}
}
// NB: on deserialization, this function is not called after the component is reset.
// So anything that happens here should be safely serialized.
virtual void SetBounds(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1)
{
m_WorldX0 = x0;
m_WorldZ0 = z0;
m_WorldX1 = x1;
m_WorldZ1 = z1;
MakeDirtyAll();
// Subdivision system bounds:
ENSURE(x0.IsZero() && z0.IsZero()); // don't bother implementing non-zero offsets yet
ResetSubdivisions(x1, z1);
CmpPtr cmpTerrain(GetSystemEntity());
if (!cmpTerrain)
return;
m_TerrainTiles = cmpTerrain->GetTilesPerSide();
m_UpdateInformations.dirtinessGrid = Grid(m_TerrainTiles*Pathfinding::NAVCELLS_PER_TILE, m_TerrainTiles*Pathfinding::NAVCELLS_PER_TILE);
CmpPtr cmpPathfinder(GetSystemEntity());
if (cmpPathfinder)
m_MaxClearance = cmpPathfinder->GetMaximumClearance();
}
void ResetSubdivisions(entity_pos_t x1, entity_pos_t z1)
{
// Use 8x8 tile subdivisions
// (TODO: find the optimal number instead of blindly guessing)
m_UnitSubdivision.Reset(x1, z1, entity_pos_t::FromInt(8*TERRAIN_TILE_SIZE));
m_StaticSubdivision.Reset(x1, z1, entity_pos_t::FromInt(8*TERRAIN_TILE_SIZE));
for (std::map::iterator it = m_UnitShapes.begin(); it != m_UnitShapes.end(); ++it)
{
CFixedVector2D center(it->second.x, it->second.z);
CFixedVector2D halfSize(it->second.clearance, it->second.clearance);
m_UnitSubdivision.Add(it->first, center - halfSize, center + halfSize);
}
for (std::map::iterator it = m_StaticShapes.begin(); it != m_StaticShapes.end(); ++it)
{
CFixedVector2D center(it->second.x, it->second.z);
CFixedVector2D bbHalfSize = Geometry::GetHalfBoundingBox(it->second.u, it->second.v, CFixedVector2D(it->second.hw, it->second.hh));
m_StaticSubdivision.Add(it->first, center - bbHalfSize, center + bbHalfSize);
}
}
virtual tag_t AddUnitShape(entity_id_t ent, entity_pos_t x, entity_pos_t z, entity_pos_t clearance, flags_t flags, entity_id_t group)
{
UnitShape shape = { ent, x, z, clearance, flags, group };
u32 id = m_UnitShapeNext++;
m_UnitShapes[id] = shape;
m_UnitSubdivision.Add(id, CFixedVector2D(x - clearance, z - clearance), CFixedVector2D(x + clearance, z + clearance));
MakeDirtyUnit(flags, id, shape);
return UNIT_INDEX_TO_TAG(id);
}
virtual tag_t AddStaticShape(entity_id_t ent, entity_pos_t x, entity_pos_t z, entity_angle_t a, entity_pos_t w, entity_pos_t h, flags_t flags, entity_id_t group, entity_id_t group2 /* = INVALID_ENTITY */)
{
fixed s, c;
sincos_approx(a, s, c);
CFixedVector2D u(c, -s);
CFixedVector2D v(s, c);
StaticShape shape = { ent, x, z, u, v, w/2, h/2, flags, group, group2 };
u32 id = m_StaticShapeNext++;
m_StaticShapes[id] = shape;
CFixedVector2D center(x, z);
CFixedVector2D bbHalfSize = Geometry::GetHalfBoundingBox(u, v, CFixedVector2D(w/2, h/2));
m_StaticSubdivision.Add(id, center - bbHalfSize, center + bbHalfSize);
MakeDirtyStatic(flags, id, shape);
return STATIC_INDEX_TO_TAG(id);
}
virtual ObstructionSquare GetUnitShapeObstruction(entity_pos_t x, entity_pos_t z, entity_pos_t clearance) const
{
CFixedVector2D u(entity_pos_t::FromInt(1), entity_pos_t::Zero());
CFixedVector2D v(entity_pos_t::Zero(), entity_pos_t::FromInt(1));
ObstructionSquare o = { x, z, u, v, clearance, clearance };
return o;
}
virtual ObstructionSquare GetStaticShapeObstruction(entity_pos_t x, entity_pos_t z, entity_angle_t a, entity_pos_t w, entity_pos_t h) const
{
fixed s, c;
sincos_approx(a, s, c);
CFixedVector2D u(c, -s);
CFixedVector2D v(s, c);
ObstructionSquare o = { x, z, u, v, w/2, h/2 };
return o;
}
virtual void MoveShape(tag_t tag, entity_pos_t x, entity_pos_t z, entity_angle_t a)
{
ENSURE(TAG_IS_VALID(tag));
if (TAG_IS_UNIT(tag))
{
UnitShape& shape = m_UnitShapes[TAG_TO_INDEX(tag)];
MakeDirtyUnit(shape.flags, TAG_TO_INDEX(tag), shape); // dirty the old shape region
m_UnitSubdivision.Move(TAG_TO_INDEX(tag),
CFixedVector2D(shape.x - shape.clearance, shape.z - shape.clearance),
CFixedVector2D(shape.x + shape.clearance, shape.z + shape.clearance),
CFixedVector2D(x - shape.clearance, z - shape.clearance),
CFixedVector2D(x + shape.clearance, z + shape.clearance));
shape.x = x;
shape.z = z;
MakeDirtyUnit(shape.flags, TAG_TO_INDEX(tag), shape); // dirty the new shape region
}
else
{
fixed s, c;
sincos_approx(a, s, c);
CFixedVector2D u(c, -s);
CFixedVector2D v(s, c);
StaticShape& shape = m_StaticShapes[TAG_TO_INDEX(tag)];
MakeDirtyStatic(shape.flags, TAG_TO_INDEX(tag), shape); // dirty the old shape region
CFixedVector2D fromBbHalfSize = Geometry::GetHalfBoundingBox(shape.u, shape.v, CFixedVector2D(shape.hw, shape.hh));
CFixedVector2D toBbHalfSize = Geometry::GetHalfBoundingBox(u, v, CFixedVector2D(shape.hw, shape.hh));
m_StaticSubdivision.Move(TAG_TO_INDEX(tag),
CFixedVector2D(shape.x, shape.z) - fromBbHalfSize,
CFixedVector2D(shape.x, shape.z) + fromBbHalfSize,
CFixedVector2D(x, z) - toBbHalfSize,
CFixedVector2D(x, z) + toBbHalfSize);
shape.x = x;
shape.z = z;
shape.u = u;
shape.v = v;
MakeDirtyStatic(shape.flags, TAG_TO_INDEX(tag), shape); // dirty the new shape region
}
}
virtual void SetUnitMovingFlag(tag_t tag, bool moving)
{
ENSURE(TAG_IS_VALID(tag) && TAG_IS_UNIT(tag));
if (TAG_IS_UNIT(tag))
{
UnitShape& shape = m_UnitShapes[TAG_TO_INDEX(tag)];
if (moving)
shape.flags |= FLAG_MOVING;
else
shape.flags &= (flags_t)~FLAG_MOVING;
MakeDirtyDebug();
}
}
virtual void SetUnitControlGroup(tag_t tag, entity_id_t group)
{
ENSURE(TAG_IS_VALID(tag) && TAG_IS_UNIT(tag));
if (TAG_IS_UNIT(tag))
{
UnitShape& shape = m_UnitShapes[TAG_TO_INDEX(tag)];
shape.group = group;
}
}
virtual void SetStaticControlGroup(tag_t tag, entity_id_t group, entity_id_t group2)
{
ENSURE(TAG_IS_VALID(tag) && TAG_IS_STATIC(tag));
if (TAG_IS_STATIC(tag))
{
StaticShape& shape = m_StaticShapes[TAG_TO_INDEX(tag)];
shape.group = group;
shape.group2 = group2;
}
}
virtual void RemoveShape(tag_t tag)
{
ENSURE(TAG_IS_VALID(tag));
if (TAG_IS_UNIT(tag))
{
UnitShape& shape = m_UnitShapes[TAG_TO_INDEX(tag)];
m_UnitSubdivision.Remove(TAG_TO_INDEX(tag),
CFixedVector2D(shape.x - shape.clearance, shape.z - shape.clearance),
CFixedVector2D(shape.x + shape.clearance, shape.z + shape.clearance));
MakeDirtyUnit(shape.flags, TAG_TO_INDEX(tag), shape);
m_UnitShapes.erase(TAG_TO_INDEX(tag));
}
else
{
StaticShape& shape = m_StaticShapes[TAG_TO_INDEX(tag)];
CFixedVector2D center(shape.x, shape.z);
CFixedVector2D bbHalfSize = Geometry::GetHalfBoundingBox(shape.u, shape.v, CFixedVector2D(shape.hw, shape.hh));
m_StaticSubdivision.Remove(TAG_TO_INDEX(tag), center - bbHalfSize, center + bbHalfSize);
MakeDirtyStatic(shape.flags, TAG_TO_INDEX(tag), shape);
m_StaticShapes.erase(TAG_TO_INDEX(tag));
}
}
virtual ObstructionSquare GetObstruction(tag_t tag) const
{
ENSURE(TAG_IS_VALID(tag));
if (TAG_IS_UNIT(tag))
{
const UnitShape& shape = m_UnitShapes.at(TAG_TO_INDEX(tag));
CFixedVector2D u(entity_pos_t::FromInt(1), entity_pos_t::Zero());
CFixedVector2D v(entity_pos_t::Zero(), entity_pos_t::FromInt(1));
ObstructionSquare o = { shape.x, shape.z, u, v, shape.clearance, shape.clearance };
return o;
}
else
{
const StaticShape& shape = m_StaticShapes.at(TAG_TO_INDEX(tag));
ObstructionSquare o = { shape.x, shape.z, shape.u, shape.v, shape.hw, shape.hh };
return o;
}
}
virtual fixed DistanceToPoint(entity_id_t ent, entity_pos_t px, entity_pos_t pz) const;
virtual fixed MaxDistanceToPoint(entity_id_t ent, entity_pos_t px, entity_pos_t pz) const;
virtual fixed DistanceToTarget(entity_id_t ent, entity_id_t target) const;
virtual fixed MaxDistanceToTarget(entity_id_t ent, entity_id_t target) const;
virtual fixed DistanceBetweenShapes(const ObstructionSquare& source, const ObstructionSquare& target) const;
virtual fixed MaxDistanceBetweenShapes(const ObstructionSquare& source, const ObstructionSquare& target) const;
virtual bool IsInPointRange(entity_id_t ent, entity_pos_t px, entity_pos_t pz, entity_pos_t minRange, entity_pos_t maxRange, bool opposite) const;
virtual bool IsInTargetRange(entity_id_t ent, entity_id_t target, entity_pos_t minRange, entity_pos_t maxRange, bool opposite) const;
virtual bool IsPointInPointRange(entity_pos_t x, entity_pos_t z, entity_pos_t px, entity_pos_t pz, entity_pos_t minRange, entity_pos_t maxRange) const;
virtual bool AreShapesInRange(const ObstructionSquare& source, const ObstructionSquare& target, entity_pos_t minRange, entity_pos_t maxRange, bool opposite) const;
virtual bool TestLine(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, bool relaxClearanceForUnits = false) const;
virtual bool TestStaticShape(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, std::vector* out) const;
virtual bool TestUnitShape(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, std::vector* out) const;
virtual void Rasterize(Grid& grid, const std::vector& passClasses, bool fullUpdate);
virtual void GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const;
virtual void GetUnitObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const;
virtual void GetStaticObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const;
virtual void GetUnitsOnObstruction(const ObstructionSquare& square, std::vector& out, const IObstructionTestFilter& filter, bool strict = false) const;
virtual void GetStaticObstructionsOnObstruction(const ObstructionSquare& square, std::vector& out, const IObstructionTestFilter& filter) const;
virtual void SetPassabilityCircular(bool enabled)
{
m_PassabilityCircular = enabled;
MakeDirtyAll();
CMessageObstructionMapShapeChanged msg;
GetSimContext().GetComponentManager().BroadcastMessage(msg);
}
virtual bool GetPassabilityCircular() const
{
return m_PassabilityCircular;
}
virtual void SetDebugOverlay(bool enabled)
{
m_DebugOverlayEnabled = enabled;
m_DebugOverlayDirty = true;
if (!enabled)
m_DebugOverlayLines.clear();
}
void RenderSubmit(SceneCollector& collector);
virtual void UpdateInformations(GridUpdateInformation& informations)
{
if (!m_UpdateInformations.dirtinessGrid.blank())
informations.MergeAndClear(m_UpdateInformations);
}
private:
// Dynamic updates for the long-range pathfinder
GridUpdateInformation m_UpdateInformations;
// These vectors might contain shapes that were deleted
std::vector m_DirtyStaticShapes;
std::vector m_DirtyUnitShapes;
/**
* Mark all previous Rasterize()d grids as dirty, and the debug display.
* Call this when the world bounds have changed.
*/
void MakeDirtyAll()
{
m_UpdateInformations.dirty = true;
m_UpdateInformations.globallyDirty = true;
m_UpdateInformations.dirtinessGrid.reset();
m_DebugOverlayDirty = true;
}
/**
* Mark the debug display as dirty.
* Call this when nothing has changed except a unit's 'moving' flag.
*/
void MakeDirtyDebug()
{
m_DebugOverlayDirty = true;
}
inline void MarkDirtinessGrid(const entity_pos_t& x, const entity_pos_t& z, const entity_pos_t& r)
{
MarkDirtinessGrid(x, z, CFixedVector2D(r, r));
}
inline void MarkDirtinessGrid(const entity_pos_t& x, const entity_pos_t& z, const CFixedVector2D& hbox)
{
ENSURE(m_UpdateInformations.dirtinessGrid.m_W == m_TerrainTiles*Pathfinding::NAVCELLS_PER_TILE &&
m_UpdateInformations.dirtinessGrid.m_H == m_TerrainTiles*Pathfinding::NAVCELLS_PER_TILE);
if (m_TerrainTiles == 0)
return;
u16 j0, j1, i0, i1;
Pathfinding::NearestNavcell(x - hbox.X, z - hbox.Y, i0, j0, m_UpdateInformations.dirtinessGrid.m_W, m_UpdateInformations.dirtinessGrid.m_H);
Pathfinding::NearestNavcell(x + hbox.X, z + hbox.Y, i1, j1, m_UpdateInformations.dirtinessGrid.m_W, m_UpdateInformations.dirtinessGrid.m_H);
for (int j = j0; j < j1; ++j)
for (int i = i0; i < i1; ++i)
m_UpdateInformations.dirtinessGrid.set(i, j, 1);
}
/**
* Mark all previous Rasterize()d grids as dirty, if they depend on this shape.
* Call this when a static shape has changed.
*/
void MakeDirtyStatic(flags_t flags, u32 index, const StaticShape& shape)
{
m_DebugOverlayDirty = true;
if (flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION))
{
m_UpdateInformations.dirty = true;
if (std::find(m_DirtyStaticShapes.begin(), m_DirtyStaticShapes.end(), index) == m_DirtyStaticShapes.end())
m_DirtyStaticShapes.push_back(index);
// All shapes overlapping the updated part of the grid should be dirtied too.
// We are going to invalidate the region of the grid corresponding to the modified shape plus its clearance,
// and we need to get the shapes whose clearance can overlap this area. So we need to extend the search area
// by two times the maximum clearance.
CFixedVector2D center(shape.x, shape.z);
CFixedVector2D hbox = Geometry::GetHalfBoundingBox(shape.u, shape.v, CFixedVector2D(shape.hw, shape.hh));
CFixedVector2D expand(m_MaxClearance, m_MaxClearance);
std::vector staticsNear;
m_StaticSubdivision.GetInRange(staticsNear, center - hbox - expand*2, center + hbox + expand*2);
for (u32& staticId : staticsNear)
if (std::find(m_DirtyStaticShapes.begin(), m_DirtyStaticShapes.end(), staticId) == m_DirtyStaticShapes.end())
m_DirtyStaticShapes.push_back(staticId);
std::vector unitsNear;
m_UnitSubdivision.GetInRange(unitsNear, center - hbox - expand*2, center + hbox + expand*2);
for (u32& unitId : unitsNear)
if (std::find(m_DirtyUnitShapes.begin(), m_DirtyUnitShapes.end(), unitId) == m_DirtyUnitShapes.end())
m_DirtyUnitShapes.push_back(unitId);
MarkDirtinessGrid(shape.x, shape.z, hbox + expand);
}
}
/**
* Mark all previous Rasterize()d grids as dirty, if they depend on this shape.
* Call this when a unit shape has changed.
*/
void MakeDirtyUnit(flags_t flags, u32 index, const UnitShape& shape)
{
m_DebugOverlayDirty = true;
if (flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION))
{
m_UpdateInformations.dirty = true;
if (std::find(m_DirtyUnitShapes.begin(), m_DirtyUnitShapes.end(), index) == m_DirtyUnitShapes.end())
m_DirtyUnitShapes.push_back(index);
// All shapes overlapping the updated part of the grid should be dirtied too.
// We are going to invalidate the region of the grid corresponding to the modified shape plus its clearance,
// and we need to get the shapes whose clearance can overlap this area. So we need to extend the search area
// by two times the maximum clearance.
CFixedVector2D center(shape.x, shape.z);
std::vector staticsNear;
m_StaticSubdivision.GetNear(staticsNear, center, shape.clearance + m_MaxClearance*2);
for (u32& staticId : staticsNear)
if (std::find(m_DirtyStaticShapes.begin(), m_DirtyStaticShapes.end(), staticId) == m_DirtyStaticShapes.end())
m_DirtyStaticShapes.push_back(staticId);
std::vector unitsNear;
m_UnitSubdivision.GetNear(unitsNear, center, shape.clearance + m_MaxClearance*2);
for (u32& unitId : unitsNear)
if (std::find(m_DirtyUnitShapes.begin(), m_DirtyUnitShapes.end(), unitId) == m_DirtyUnitShapes.end())
m_DirtyUnitShapes.push_back(unitId);
MarkDirtinessGrid(shape.x, shape.z, shape.clearance + m_MaxClearance);
}
}
/**
* Return whether the given point is within the world bounds by at least r
*/
inline bool IsInWorld(entity_pos_t x, entity_pos_t z, entity_pos_t r) const
{
return (m_WorldX0+r <= x && x <= m_WorldX1-r && m_WorldZ0+r <= z && z <= m_WorldZ1-r);
}
/**
* Return whether the given point is within the world bounds
*/
inline bool IsInWorld(const CFixedVector2D& p) const
{
return (m_WorldX0 <= p.X && p.X <= m_WorldX1 && m_WorldZ0 <= p.Y && p.Y <= m_WorldZ1);
}
void RasterizeHelper(Grid& grid, ICmpObstructionManager::flags_t requireMask, bool fullUpdate, pass_class_t appliedMask, entity_pos_t clearance = fixed::Zero()) const;
};
REGISTER_COMPONENT_TYPE(ObstructionManager)
/**
* DistanceTo function family, all end up in calculating a vector length, DistanceBetweenShapes or
* MaxDistanceBetweenShapes. The MaxFoo family calculates the opposite edge opposite edge distance.
* When the distance is undefined we return -1.
*/
fixed CCmpObstructionManager::DistanceToPoint(entity_id_t ent, entity_pos_t px, entity_pos_t pz) const
{
ObstructionSquare obstruction;
CmpPtr cmpObstruction(GetSimContext(), ent);
if (cmpObstruction && cmpObstruction->GetObstructionSquare(obstruction))
{
ObstructionSquare point;
point.x = px;
point.z = pz;
return DistanceBetweenShapes(obstruction, point);
}
CmpPtr cmpPosition(GetSimContext(), ent);
if (!cmpPosition || !cmpPosition->IsInWorld())
return fixed::FromInt(-1);
return (CFixedVector2D(cmpPosition->GetPosition2D().X, cmpPosition->GetPosition2D().Y) - CFixedVector2D(px, pz)).Length();
}
fixed CCmpObstructionManager::MaxDistanceToPoint(entity_id_t ent, entity_pos_t px, entity_pos_t pz) const
{
ObstructionSquare obstruction;
CmpPtr cmpObstruction(GetSimContext(), ent);
if (!cmpObstruction || !cmpObstruction->GetObstructionSquare(obstruction))
{
ObstructionSquare point;
point.x = px;
point.z = pz;
return MaxDistanceBetweenShapes(obstruction, point);
}
CmpPtr cmpPosition(GetSimContext(), ent);
if (!cmpPosition || !cmpPosition->IsInWorld())
return fixed::FromInt(-1);
return (CFixedVector2D(cmpPosition->GetPosition2D().X, cmpPosition->GetPosition2D().Y) - CFixedVector2D(px, pz)).Length();
}
fixed CCmpObstructionManager::DistanceToTarget(entity_id_t ent, entity_id_t target) const
{
ObstructionSquare obstruction;
CmpPtr cmpObstruction(GetSimContext(), ent);
if (!cmpObstruction || !cmpObstruction->GetObstructionSquare(obstruction))
{
CmpPtr cmpPosition(GetSimContext(), ent);
if (!cmpPosition || !cmpPosition->IsInWorld())
return fixed::FromInt(-1);
return DistanceToPoint(target, cmpPosition->GetPosition2D().X, cmpPosition->GetPosition2D().Y);
}
ObstructionSquare target_obstruction;
CmpPtr cmpObstructionTarget(GetSimContext(), target);
if (!cmpObstructionTarget || !cmpObstructionTarget->GetObstructionSquare(target_obstruction))
{
CmpPtr cmpPositionTarget(GetSimContext(), target);
if (!cmpPositionTarget || !cmpPositionTarget->IsInWorld())
return fixed::FromInt(-1);
return DistanceToPoint(ent, cmpPositionTarget->GetPosition2D().X, cmpPositionTarget->GetPosition2D().Y);
}
return DistanceBetweenShapes(obstruction, target_obstruction);
}
fixed CCmpObstructionManager::MaxDistanceToTarget(entity_id_t ent, entity_id_t target) const
{
ObstructionSquare obstruction;
CmpPtr cmpObstruction(GetSimContext(), ent);
if (!cmpObstruction || !cmpObstruction->GetObstructionSquare(obstruction))
{
CmpPtr cmpPosition(GetSimContext(), ent);
if (!cmpPosition || !cmpPosition->IsInWorld())
return fixed::FromInt(-1);
return MaxDistanceToPoint(target, cmpPosition->GetPosition2D().X, cmpPosition->GetPosition2D().Y);
}
ObstructionSquare target_obstruction;
CmpPtr cmpObstructionTarget(GetSimContext(), target);
if (!cmpObstructionTarget || !cmpObstructionTarget->GetObstructionSquare(target_obstruction))
{
CmpPtr cmpPositionTarget(GetSimContext(), target);
if (!cmpPositionTarget || !cmpPositionTarget->IsInWorld())
return fixed::FromInt(-1);
return MaxDistanceToPoint(ent, cmpPositionTarget->GetPosition2D().X, cmpPositionTarget->GetPosition2D().Y);
}
return MaxDistanceBetweenShapes(obstruction, target_obstruction);
}
fixed CCmpObstructionManager::DistanceBetweenShapes(const ObstructionSquare& source, const ObstructionSquare& target) const
{
// Sphere-sphere collision.
if (source.hh == fixed::Zero() && target.hh == fixed::Zero())
return (CFixedVector2D(target.x, target.z) - CFixedVector2D(source.x, source.z)).Length() - source.hw - target.hw;
// Square to square.
if (source.hh != fixed::Zero() && target.hh != fixed::Zero())
return Geometry::DistanceSquareToSquare(
CFixedVector2D(target.x, target.z) - CFixedVector2D(source.x, source.z),
source.u, source.v, CFixedVector2D(source.hw, source.hh),
target.u, target.v, CFixedVector2D(target.hw, target.hh));
// To cover both remaining cases, shape a is the square one, shape b is the circular one.
const ObstructionSquare& a = source.hh == fixed::Zero() ? target : source;
const ObstructionSquare& b = source.hh == fixed::Zero() ? source : target;
return Geometry::DistanceToSquare(
CFixedVector2D(b.x, b.z) - CFixedVector2D(a.x, a.z),
a.u, a.v, CFixedVector2D(a.hw, a.hh), true) - b.hw;
}
fixed CCmpObstructionManager::MaxDistanceBetweenShapes(const ObstructionSquare& source, const ObstructionSquare& target) const
{
// Sphere-sphere collision.
if (source.hh == fixed::Zero() && target.hh == fixed::Zero())
return (CFixedVector2D(target.x, target.z) - CFixedVector2D(source.x, source.z)).Length() + source.hw + target.hw;
// Square to square.
if (source.hh != fixed::Zero() && target.hh != fixed::Zero())
return Geometry::MaxDistanceSquareToSquare(
CFixedVector2D(target.x, target.z) - CFixedVector2D(source.x, source.z),
source.u, source.v, CFixedVector2D(source.hw, source.hh),
target.u, target.v, CFixedVector2D(target.hw, target.hh));
// To cover both remaining cases, shape a is the square one, shape b is the circular one.
const ObstructionSquare& a = source.hh == fixed::Zero() ? target : source;
const ObstructionSquare& b = source.hh == fixed::Zero() ? source : target;
return Geometry::MaxDistanceToSquare(
CFixedVector2D(b.x, b.z) - CFixedVector2D(a.x, a.z),
a.u, a.v, CFixedVector2D(a.hw, a.hh), true) + b.hw;
}
/**
* IsInRange function family depending on the DistanceTo family.
*
* In range if the edge to edge distance is inferior to maxRange
* and if the opposite edge to opposite edge distance is greater than minRange when the opposite bool is true
* or when the opposite bool is false the edge to edge distance is more than minRange.
*
* Using the opposite egde for minRange means that a unit is in range of a building if it is farther than
* clearance-buildingsize, which is generally going to be negative (and thus this returns true).
* NB: from a game POV, this means units can easily fire on buildings, which is good,
* but it also means that buildings can easily fire on units. Buildings are usually meant
* to fire from the edge, not the opposite edge, so this looks odd. For this reason one can choose
* to set the opposite bool false and use the edge to egde distance.
*
* We don't use squares because the are likely to overflow.
* We use a 0.0001 margin to avoid rounding errors.
*/
bool CCmpObstructionManager::IsInPointRange(entity_id_t ent, entity_pos_t px, entity_pos_t pz, entity_pos_t minRange, entity_pos_t maxRange, bool opposite) const
{
fixed dist = DistanceToPoint(ent, px, pz);
// Treat -1 max range as infinite
return dist != fixed::FromInt(-1) &&
(dist <= (maxRange + fixed::FromFloat(0.0001)) || maxRange < fixed::Zero()) &&
(opposite ? MaxDistanceToPoint(ent, px, pz) : dist) >= minRange - fixed::FromFloat(0.0001);
}
bool CCmpObstructionManager::IsInTargetRange(entity_id_t ent, entity_id_t target, entity_pos_t minRange, entity_pos_t maxRange, bool opposite) const
{
fixed dist = DistanceToTarget(ent, target);
// Treat -1 max range as infinite
return dist != fixed::FromInt(-1) &&
(dist <= (maxRange + fixed::FromFloat(0.0001)) || maxRange < fixed::Zero()) &&
(opposite ? MaxDistanceToTarget(ent, target) : dist) >= minRange - fixed::FromFloat(0.0001);
}
bool CCmpObstructionManager::IsPointInPointRange(entity_pos_t x, entity_pos_t z, entity_pos_t px, entity_pos_t pz, entity_pos_t minRange, entity_pos_t maxRange) const
{
entity_pos_t distance = (CFixedVector2D(x, z) - CFixedVector2D(px, pz)).Length();
// Treat -1 max range as infinite
return (distance <= (maxRange + fixed::FromFloat(0.0001)) || maxRange < fixed::Zero()) &&
distance >= minRange - fixed::FromFloat(0.0001);
}
bool CCmpObstructionManager::AreShapesInRange(const ObstructionSquare& source, const ObstructionSquare& target, entity_pos_t minRange, entity_pos_t maxRange, bool opposite) const
{
fixed dist = DistanceBetweenShapes(source, target);
// Treat -1 max range as infinite
return dist != fixed::FromInt(-1) &&
(dist <= (maxRange + fixed::FromFloat(0.0001)) || maxRange < fixed::Zero()) &&
(opposite ? MaxDistanceBetweenShapes(source, target) : dist) >= minRange - fixed::FromFloat(0.0001);
}
bool CCmpObstructionManager::TestLine(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, bool relaxClearanceForUnits) const
{
PROFILE("TestLine");
// Check that both end points are within the world (which means the whole line must be)
if (!IsInWorld(x0, z0, r) || !IsInWorld(x1, z1, r))
return true;
CFixedVector2D posMin (std::min(x0, x1) - r, std::min(z0, z1) - r);
CFixedVector2D posMax (std::max(x0, x1) + r, std::max(z0, z1) + r);
// actual radius used for unit-unit collisions. If relaxClearanceForUnits, will be smaller to allow more overlap.
entity_pos_t unitUnitRadius = r;
if (relaxClearanceForUnits)
unitUnitRadius -= entity_pos_t::FromInt(1)/2;
std::vector unitShapes;
m_UnitSubdivision.GetInRange(unitShapes, posMin, posMax);
for (const entity_id_t& shape : unitShapes)
{
std::map::const_iterator it = m_UnitShapes.find(shape);
ENSURE(it != m_UnitShapes.end());
if (!filter.TestShape(UNIT_INDEX_TO_TAG(it->first), it->second.flags, it->second.group, INVALID_ENTITY))
continue;
CFixedVector2D center(it->second.x, it->second.z);
CFixedVector2D halfSize(it->second.clearance + unitUnitRadius, it->second.clearance + unitUnitRadius);
if (Geometry::TestRayAASquare(CFixedVector2D(x0, z0) - center, CFixedVector2D(x1, z1) - center, halfSize))
return true;
}
std::vector staticShapes;
m_StaticSubdivision.GetInRange(staticShapes, posMin, posMax);
for (const entity_id_t& shape : staticShapes)
{
std::map::const_iterator it = m_StaticShapes.find(shape);
ENSURE(it != m_StaticShapes.end());
if (!filter.TestShape(STATIC_INDEX_TO_TAG(it->first), it->second.flags, it->second.group, it->second.group2))
continue;
CFixedVector2D center(it->second.x, it->second.z);
CFixedVector2D halfSize(it->second.hw + r, it->second.hh + r);
if (Geometry::TestRaySquare(CFixedVector2D(x0, z0) - center, CFixedVector2D(x1, z1) - center, it->second.u, it->second.v, halfSize))
return true;
}
return false;
}
bool CCmpObstructionManager::TestStaticShape(const IObstructionTestFilter& filter,
entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h,
std::vector* out) const
{
PROFILE("TestStaticShape");
if (out)
out->clear();
fixed s, c;
sincos_approx(a, s, c);
CFixedVector2D u(c, -s);
CFixedVector2D v(s, c);
CFixedVector2D center(x, z);
CFixedVector2D halfSize(w/2, h/2);
CFixedVector2D corner1 = u.Multiply(halfSize.X) + v.Multiply(halfSize.Y);
CFixedVector2D corner2 = u.Multiply(halfSize.X) - v.Multiply(halfSize.Y);
// Check that all corners are within the world (which means the whole shape must be)
if (!IsInWorld(center + corner1) || !IsInWorld(center + corner2) ||
!IsInWorld(center - corner1) || !IsInWorld(center - corner2))
{
if (out)
out->push_back(INVALID_ENTITY); // no entity ID, so just push an arbitrary marker
else
return true;
}
fixed bbHalfWidth = std::max(corner1.X.Absolute(), corner2.X.Absolute());
fixed bbHalfHeight = std::max(corner1.Y.Absolute(), corner2.Y.Absolute());
CFixedVector2D posMin(x - bbHalfWidth, z - bbHalfHeight);
CFixedVector2D posMax(x + bbHalfWidth, z + bbHalfHeight);
std::vector unitShapes;
m_UnitSubdivision.GetInRange(unitShapes, posMin, posMax);
for (entity_id_t& shape : unitShapes)
{
std::map::const_iterator it = m_UnitShapes.find(shape);
ENSURE(it != m_UnitShapes.end());
if (!filter.TestShape(UNIT_INDEX_TO_TAG(it->first), it->second.flags, it->second.group, INVALID_ENTITY))
continue;
CFixedVector2D center1(it->second.x, it->second.z);
if (Geometry::PointIsInSquare(center1 - center, u, v, CFixedVector2D(halfSize.X + it->second.clearance, halfSize.Y + it->second.clearance)))
{
if (out)
out->push_back(it->second.entity);
else
return true;
}
}
std::vector staticShapes;
m_StaticSubdivision.GetInRange(staticShapes, posMin, posMax);
for (entity_id_t& shape : staticShapes)
{
std::map::const_iterator it = m_StaticShapes.find(shape);
ENSURE(it != m_StaticShapes.end());
if (!filter.TestShape(STATIC_INDEX_TO_TAG(it->first), it->second.flags, it->second.group, it->second.group2))
continue;
CFixedVector2D center1(it->second.x, it->second.z);
CFixedVector2D halfSize1(it->second.hw, it->second.hh);
if (Geometry::TestSquareSquare(center, u, v, halfSize, center1, it->second.u, it->second.v, halfSize1))
{
if (out)
out->push_back(it->second.entity);
else
return true;
}
}
if (out)
return !out->empty(); // collided if the list isn't empty
else
return false; // didn't collide, if we got this far
}
bool CCmpObstructionManager::TestUnitShape(const IObstructionTestFilter& filter,
entity_pos_t x, entity_pos_t z, entity_pos_t clearance,
std::vector* out) const
{
PROFILE("TestUnitShape");
// Check that the shape is within the world
if (!IsInWorld(x, z, clearance))
{
if (out)
out->push_back(INVALID_ENTITY); // no entity ID, so just push an arbitrary marker
else
return true;
}
CFixedVector2D center(x, z);
CFixedVector2D posMin(x - clearance, z - clearance);
CFixedVector2D posMax(x + clearance, z + clearance);
std::vector unitShapes;
m_UnitSubdivision.GetInRange(unitShapes, posMin, posMax);
for (const entity_id_t& shape : unitShapes)
{
std::map::const_iterator it = m_UnitShapes.find(shape);
ENSURE(it != m_UnitShapes.end());
if (!filter.TestShape(UNIT_INDEX_TO_TAG(it->first), it->second.flags, it->second.group, INVALID_ENTITY))
continue;
entity_pos_t c1 = it->second.clearance;
if (!(
it->second.x + c1 < x - clearance ||
it->second.x - c1 > x + clearance ||
it->second.z + c1 < z - clearance ||
it->second.z - c1 > z + clearance))
{
if (out)
out->push_back(it->second.entity);
else
return true;
}
}
std::vector staticShapes;
m_StaticSubdivision.GetInRange(staticShapes, posMin, posMax);
for (const entity_id_t& shape : staticShapes)
{
std::map::const_iterator it = m_StaticShapes.find(shape);
ENSURE(it != m_StaticShapes.end());
if (!filter.TestShape(STATIC_INDEX_TO_TAG(it->first), it->second.flags, it->second.group, it->second.group2))
continue;
CFixedVector2D center1(it->second.x, it->second.z);
if (Geometry::PointIsInSquare(center1 - center, it->second.u, it->second.v, CFixedVector2D(it->second.hw + clearance, it->second.hh + clearance)))
{
if (out)
out->push_back(it->second.entity);
else
return true;
}
}
if (out)
return !out->empty(); // collided if the list isn't empty
else
return false; // didn't collide, if we got this far
}
void CCmpObstructionManager::Rasterize(Grid& grid, const std::vector& passClasses, bool fullUpdate)
{
PROFILE3("Rasterize Obstructions");
// Cells are only marked as blocked if the whole cell is strictly inside the shape.
// (That ensures the shape's geometric border is always reachable.)
// Pass classes will get shapes rasterized on them depending on their Obstruction value.
// Classes with another value than "pathfinding" should not use Clearance.
std::map pathfindingMasks;
u16 foundationMask = 0;
for (const PathfinderPassability& passability : passClasses)
{
switch (passability.m_Obstructions)
{
case PathfinderPassability::PATHFINDING:
{
std::map::iterator it = pathfindingMasks.find(passability.m_Clearance);
if (it == pathfindingMasks.end())
pathfindingMasks[passability.m_Clearance] = passability.m_Mask;
else
it->second |= passability.m_Mask;
break;
}
case PathfinderPassability::FOUNDATION:
foundationMask |= passability.m_Mask;
break;
default:
continue;
}
}
// FLAG_BLOCK_PATHFINDING and FLAG_BLOCK_FOUNDATION are the only flags taken into account by MakeDirty* functions,
// so they should be the only ones rasterized using with the help of m_Dirty*Shapes vectors.
for (auto& maskPair : pathfindingMasks)
RasterizeHelper(grid, FLAG_BLOCK_PATHFINDING, fullUpdate, maskPair.second, maskPair.first);
RasterizeHelper(grid, FLAG_BLOCK_FOUNDATION, fullUpdate, foundationMask);
m_DirtyStaticShapes.clear();
m_DirtyUnitShapes.clear();
}
void CCmpObstructionManager::RasterizeHelper(Grid& grid, ICmpObstructionManager::flags_t requireMask, bool fullUpdate, pass_class_t appliedMask, entity_pos_t clearance) const
{
for (auto& pair : m_StaticShapes)
{
const StaticShape& shape = pair.second;
if (!(shape.flags & requireMask))
continue;
if (!fullUpdate && std::find(m_DirtyStaticShapes.begin(), m_DirtyStaticShapes.end(), pair.first) == m_DirtyStaticShapes.end())
continue;
// TODO: it might be nice to rasterize with rounded corners for large 'expand' values.
ObstructionSquare square = { shape.x, shape.z, shape.u, shape.v, shape.hw, shape.hh };
SimRasterize::Spans spans;
SimRasterize::RasterizeRectWithClearance(spans, square, clearance, Pathfinding::NAVCELL_SIZE);
for (SimRasterize::Span& span : spans)
{
i16 j = Clamp(span.j, (i16)0, (i16)(grid.m_H-1));
i16 i0 = std::max(span.i0, (i16)0);
i16 i1 = std::min(span.i1, (i16)grid.m_W);
for (i16 i = i0; i < i1; ++i)
grid.set(i, j, grid.get(i, j) | appliedMask);
}
}
for (auto& pair : m_UnitShapes)
{
if (!(pair.second.flags & requireMask))
continue;
if (!fullUpdate && std::find(m_DirtyUnitShapes.begin(), m_DirtyUnitShapes.end(), pair.first) == m_DirtyUnitShapes.end())
continue;
CFixedVector2D center(pair.second.x, pair.second.z);
entity_pos_t r = pair.second.clearance + clearance;
u16 i0, j0, i1, j1;
Pathfinding::NearestNavcell(center.X - r, center.Y - r, i0, j0, grid.m_W, grid.m_H);
Pathfinding::NearestNavcell(center.X + r, center.Y + r, i1, j1, grid.m_W, grid.m_H);
for (u16 j = j0+1; j < j1; ++j)
for (u16 i = i0+1; i < i1; ++i)
grid.set(i, j, grid.get(i, j) | appliedMask);
}
}
void CCmpObstructionManager::GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const
{
GetUnitObstructionsInRange(filter, x0, z0, x1, z1, squares);
GetStaticObstructionsInRange(filter, x0, z0, x1, z1, squares);
}
void CCmpObstructionManager::GetUnitObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const
{
PROFILE("GetObstructionsInRange");
ENSURE(x0 <= x1 && z0 <= z1);
std::vector unitShapes;
m_UnitSubdivision.GetInRange(unitShapes, CFixedVector2D(x0, z0), CFixedVector2D(x1, z1));
for (entity_id_t& unitShape : unitShapes)
{
std::map::const_iterator it = m_UnitShapes.find(unitShape);
ENSURE(it != m_UnitShapes.end());
if (!filter.TestShape(UNIT_INDEX_TO_TAG(it->first), it->second.flags, it->second.group, INVALID_ENTITY))
continue;
entity_pos_t c = it->second.clearance;
// Skip this object if it's completely outside the requested range
if (it->second.x + c < x0 || it->second.x - c > x1 || it->second.z + c < z0 || it->second.z - c > z1)
continue;
CFixedVector2D u(entity_pos_t::FromInt(1), entity_pos_t::Zero());
CFixedVector2D v(entity_pos_t::Zero(), entity_pos_t::FromInt(1));
squares.emplace_back(ObstructionSquare{ it->second.x, it->second.z, u, v, c, c });
}
}
void CCmpObstructionManager::GetStaticObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector& squares) const
{
PROFILE("GetObstructionsInRange");
ENSURE(x0 <= x1 && z0 <= z1);
std::vector staticShapes;
m_StaticSubdivision.GetInRange(staticShapes, CFixedVector2D(x0, z0), CFixedVector2D(x1, z1));
for (entity_id_t& staticShape : staticShapes)
{
std::map::const_iterator it = m_StaticShapes.find(staticShape);
ENSURE(it != m_StaticShapes.end());
if (!filter.TestShape(STATIC_INDEX_TO_TAG(it->first), it->second.flags, it->second.group, it->second.group2))
continue;
entity_pos_t r = it->second.hw + it->second.hh; // overestimate the max dist of an edge from the center
// Skip this object if its overestimated bounding box is completely outside the requested range
if (it->second.x + r < x0 || it->second.x - r > x1 || it->second.z + r < z0 || it->second.z - r > z1)
continue;
// TODO: maybe we should use Geometry::GetHalfBoundingBox to be more precise?
squares.emplace_back(ObstructionSquare{ it->second.x, it->second.z, it->second.u, it->second.v, it->second.hw, it->second.hh });
}
}
void CCmpObstructionManager::GetUnitsOnObstruction(const ObstructionSquare& square, std::vector& out, const IObstructionTestFilter& filter, bool strict) const
{
PROFILE("GetUnitsOnObstruction");
// In order to avoid getting units on impassable cells, we want to find all
// units subject to the RasterizeRectWithClearance of the building's shape with the
// unit's clearance covers the navcell the unit is on.
std::vector unitShapes;
CFixedVector2D center(square.x, square.z);
CFixedVector2D expandedBox =
Geometry::GetHalfBoundingBox(square.u, square.v, CFixedVector2D(square.hw, square.hh)) +
CFixedVector2D(m_MaxClearance, m_MaxClearance);
m_UnitSubdivision.GetInRange(unitShapes, center - expandedBox, center + expandedBox);
std::map rasterizedRects;
for (const u32& unitShape : unitShapes)
{
std::map::const_iterator it = m_UnitShapes.find(unitShape);
ENSURE(it != m_UnitShapes.end());
const UnitShape& shape = it->second;
if (!filter.TestShape(UNIT_INDEX_TO_TAG(unitShape), shape.flags, shape.group, INVALID_ENTITY))
continue;
if (rasterizedRects.find(shape.clearance) == rasterizedRects.end())
{
// The rasterization is an approximation of the real shapes.
// Depending on your use, you may want to be more or less strict on the rasterization,
// ie this may either return some units that aren't actually on the shape (if strict is set)
// or this may not return some units that are on the shape (if strict is not set).
// Foundations need to be non-strict, as otherwise it sometimes detects the builder units
// as being on the shape, so it orders them away.
SimRasterize::Spans& newSpans = rasterizedRects[shape.clearance];
if (strict)
SimRasterize::RasterizeRectWithClearance(newSpans, square, shape.clearance, Pathfinding::NAVCELL_SIZE);
else
SimRasterize::RasterizeRectWithClearance(newSpans, square, shape.clearance-Pathfinding::CLEARANCE_EXTENSION_RADIUS, Pathfinding::NAVCELL_SIZE);
}
SimRasterize::Spans& spans = rasterizedRects[shape.clearance];
// Check whether the unit's center is on a navcell that's in
// any of the spans
u16 i = (shape.x / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity();
u16 j = (shape.z / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity();
for (const SimRasterize::Span& span : spans)
{
if (j == span.j && span.i0 <= i && i < span.i1)
{
out.push_back(shape.entity);
break;
}
}
}
}
void CCmpObstructionManager::GetStaticObstructionsOnObstruction(const ObstructionSquare& square, std::vector& out, const IObstructionTestFilter& filter) const
{
PROFILE("GetStaticObstructionsOnObstruction");
std::vector staticShapes;
CFixedVector2D center(square.x, square.z);
CFixedVector2D expandedBox = Geometry::GetHalfBoundingBox(square.u, square.v, CFixedVector2D(square.hw, square.hh));
m_StaticSubdivision.GetInRange(staticShapes, center - expandedBox, center + expandedBox);
for (const u32& staticShape : staticShapes)
{
std::map::const_iterator it = m_StaticShapes.find(staticShape);
ENSURE(it != m_StaticShapes.end());
const StaticShape& shape = it->second;
if (!filter.TestShape(STATIC_INDEX_TO_TAG(staticShape), shape.flags, shape.group, shape.group2))
continue;
if (Geometry::TestSquareSquare(
center,
square.u,
square.v,
CFixedVector2D(square.hw, square.hh),
CFixedVector2D(shape.x, shape.z),
shape.u,
shape.v,
CFixedVector2D(shape.hw, shape.hh)))
{
out.push_back(shape.entity);
}
}
}
void CCmpObstructionManager::RenderSubmit(SceneCollector& collector)
{
if (!m_DebugOverlayEnabled)
return;
CColor defaultColor(0, 0, 1, 1);
CColor movingColor(1, 0, 1, 1);
CColor boundsColor(1, 1, 0, 1);
// If the shapes have changed, then regenerate all the overlays
if (m_DebugOverlayDirty)
{
m_DebugOverlayLines.clear();
m_DebugOverlayLines.push_back(SOverlayLine());
m_DebugOverlayLines.back().m_Color = boundsColor;
SimRender::ConstructSquareOnGround(GetSimContext(),
(m_WorldX0+m_WorldX1).ToFloat()/2.f, (m_WorldZ0+m_WorldZ1).ToFloat()/2.f,
(m_WorldX1-m_WorldX0).ToFloat(), (m_WorldZ1-m_WorldZ0).ToFloat(),
0, m_DebugOverlayLines.back(), true);
for (std::map::iterator it = m_UnitShapes.begin(); it != m_UnitShapes.end(); ++it)
{
m_DebugOverlayLines.push_back(SOverlayLine());
m_DebugOverlayLines.back().m_Color = ((it->second.flags & FLAG_MOVING) ? movingColor : defaultColor);
- SimRender::ConstructSquareOnGround(GetSimContext(), it->second.x.ToFloat(), it->second.z.ToFloat(), it->second.clearance.ToFloat()*2, it->second.clearance.ToFloat()*2, 0, m_DebugOverlayLines.back(), true);
+ SimRender::ConstructSquareOnGround(GetSimContext(), it->second.x.ToFloat(), it->second.z.ToFloat(), it->second.clearance.ToFloat(), it->second.clearance.ToFloat(), 0, m_DebugOverlayLines.back(), true);
}
for (std::map::iterator it = m_StaticShapes.begin(); it != m_StaticShapes.end(); ++it)
{
m_DebugOverlayLines.push_back(SOverlayLine());
m_DebugOverlayLines.back().m_Color = defaultColor;
float a = atan2f(it->second.v.X.ToFloat(), it->second.v.Y.ToFloat());
SimRender::ConstructSquareOnGround(GetSimContext(), it->second.x.ToFloat(), it->second.z.ToFloat(), it->second.hw.ToFloat()*2, it->second.hh.ToFloat()*2, a, m_DebugOverlayLines.back(), true);
}
m_DebugOverlayDirty = false;
}
for (size_t i = 0; i < m_DebugOverlayLines.size(); ++i)
collector.Submit(&m_DebugOverlayLines[i]);
}
Index: ps/trunk/source/simulation2/helpers/VertexPathfinder.cpp
===================================================================
--- ps/trunk/source/simulation2/helpers/VertexPathfinder.cpp (revision 22472)
+++ ps/trunk/source/simulation2/helpers/VertexPathfinder.cpp (revision 22473)
@@ -1,969 +1,972 @@
/* Copyright (C) 2019 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 .
*/
/**
* @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 "VertexPathfinder.h"
#include "lib/timer.h"
#include "ps/Profile.h"
#include "renderer/Scene.h"
#include "simulation2/components/ICmpObstructionManager.h"
#include "simulation2/helpers/PriorityQueue.h"
#include "simulation2/helpers/Render.h"
#include "simulation2/system/SimContext.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;
// 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)/16;
/**
* 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(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& 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(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& 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(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& 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(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& 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(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector& 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;
}
typedef PriorityQueueHeap VertexPriorityQueue;
/**
* Add edges and vertexes to represent the boundaries between passable and impassable
* navcells (for impassable terrain).
* Navcells i0 <= i <= i1, j0 <= j <= j1 will be considered.
*/
static void AddTerrainEdges(std::vector& edges, std::vector& vertexes,
int i0, int j0, int i1, int j1,
pass_class_t passClass, const Grid& grid)
{
// Clamp the coordinates so we won't attempt to sample outside of the grid.
// (This assumes the outermost ring of navcells (which are always impassable)
// won't have a boundary with any passable navcells. TODO: is that definitely
// safe enough?)
i0 = clamp(i0, 1, grid.m_W-2);
j0 = clamp(j0, 1, grid.m_H-2);
i1 = clamp(i1, 1, grid.m_W-2);
j1 = clamp(j1, 1, grid.m_H-2);
for (int j = j0; j <= j1; ++j)
{
for (int i = i0; i <= i1; ++i)
{
if (IS_PASSABLE(grid.get(i, j), passClass))
continue;
if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i+1, j+1), passClass))
{
Vertex vert;
vert.status = Vertex::UNEXPLORED;
vert.quadOutward = QUADRANT_ALL;
vert.quadInward = QUADRANT_BL;
vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
vertexes.push_back(vert);
}
if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i-1, j+1), passClass))
{
Vertex vert;
vert.status = Vertex::UNEXPLORED;
vert.quadOutward = QUADRANT_ALL;
vert.quadInward = QUADRANT_BR;
vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
vertexes.push_back(vert);
}
if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i+1, j-1), passClass))
{
Vertex vert;
vert.status = Vertex::UNEXPLORED;
vert.quadOutward = QUADRANT_ALL;
vert.quadInward = QUADRANT_TL;
vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
vertexes.push_back(vert);
}
if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i-1, j-1), passClass))
{
Vertex vert;
vert.status = Vertex::UNEXPLORED;
vert.quadOutward = QUADRANT_ALL;
vert.quadInward = QUADRANT_TR;
vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
vertexes.push_back(vert);
}
}
}
// XXX rewrite this stuff
std::vector segmentsR;
std::vector segmentsL;
for (int j = j0; j < j1; ++j)
{
segmentsR.clear();
segmentsL.clear();
for (int i = i0; i <= i1; ++i)
{
bool a = IS_PASSABLE(grid.get(i, j+1), passClass);
bool b = IS_PASSABLE(grid.get(i, j), passClass);
if (a && !b)
segmentsL.push_back(i);
if (b && !a)
segmentsR.push_back(i);
}
if (!segmentsR.empty())
{
segmentsR.push_back(0); // sentinel value to simplify the loop
u16 ia = segmentsR[0];
u16 ib = ia + 1;
for (size_t n = 1; n < segmentsR.size(); ++n)
{
if (segmentsR[n] == ib)
++ib;
else
{
CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
edges.emplace_back(Edge{ v0, v1 });
ia = segmentsR[n];
ib = ia + 1;
}
}
}
if (!segmentsL.empty())
{
segmentsL.push_back(0); // sentinel value to simplify the loop
u16 ia = segmentsL[0];
u16 ib = ia + 1;
for (size_t n = 1; n < segmentsL.size(); ++n)
{
if (segmentsL[n] == ib)
++ib;
else
{
CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
edges.emplace_back(Edge{ v0, v1 });
ia = segmentsL[n];
ib = ia + 1;
}
}
}
}
std::vector segmentsU;
std::vector segmentsD;
for (int i = i0; i < i1; ++i)
{
segmentsU.clear();
segmentsD.clear();
for (int j = j0; j <= j1; ++j)
{
bool a = IS_PASSABLE(grid.get(i+1, j), passClass);
bool b = IS_PASSABLE(grid.get(i, j), passClass);
if (a && !b)
segmentsU.push_back(j);
if (b && !a)
segmentsD.push_back(j);
}
if (!segmentsU.empty())
{
segmentsU.push_back(0); // sentinel value to simplify the loop
u16 ja = segmentsU[0];
u16 jb = ja + 1;
for (size_t n = 1; n < segmentsU.size(); ++n)
{
if (segmentsU[n] == jb)
++jb;
else
{
CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE);
CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE);
edges.emplace_back(Edge{ v0, v1 });
ja = segmentsU[n];
jb = ja + 1;
}
}
}
if (!segmentsD.empty())
{
segmentsD.push_back(0); // sentinel value to simplify the loop
u16 ja = segmentsD[0];
u16 jb = ja + 1;
for (size_t n = 1; n < segmentsD.size(); ++n)
{
if (segmentsD[n] == jb)
++jb;
else
{
CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE);
CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE);
edges.emplace_back(Edge{ v0, v1 });
ja = segmentsD[n];
jb = ja + 1;
}
}
}
}
}
static void SplitAAEdges(const CFixedVector2D& a,
const std::vector& edges,
const std::vector& squares,
std::vector& edgesUnaligned,
std::vector& edgesLeft, std::vector& edgesRight,
std::vector& edgesBottom, std::vector& edgesTop)
{
for (const Square& square : squares)
{
if (a.X <= square.p0.X)
edgesLeft.emplace_back(EdgeAA{ square.p0, square.p1.Y });
if (a.X >= square.p1.X)
edgesRight.emplace_back(EdgeAA{ square.p1, square.p0.Y });
if (a.Y <= square.p0.Y)
edgesBottom.emplace_back(EdgeAA{ square.p0, square.p1.X });
if (a.Y >= square.p1.Y)
edgesTop.emplace_back(EdgeAA{ square.p1, square.p0.X });
}
for (const Edge& edge : edges)
{
if (edge.p0.X == edge.p1.X)
{
if (edge.p1.Y < edge.p0.Y)
{
if (!(a.X <= edge.p0.X))
continue;
edgesLeft.emplace_back(EdgeAA{ edge.p1, edge.p0.Y });
}
else
{
if (!(a.X >= edge.p0.X))
continue;
edgesRight.emplace_back(EdgeAA{ edge.p1, edge.p0.Y });
}
}
else if (edge.p0.Y == edge.p1.Y)
{
if (edge.p0.X < edge.p1.X)
{
if (!(a.Y <= edge.p0.Y))
continue;
edgesBottom.emplace_back(EdgeAA{ edge.p0, edge.p1.X });
}
else
{
if (!(a.Y >= edge.p0.Y))
continue;
edgesTop.emplace_back(EdgeAA{ edge.p0, edge.p1.X });
}
}
else
edgesUnaligned.push_back(edge);
}
}
/**
* Functor for sorting edge-squares by approximate proximity to a fixed point.
*/
struct SquareSort
{
CFixedVector2D src;
SquareSort(CFixedVector2D src) : src(src) { }
bool operator()(const Square& a, const Square& b) const
{
if ((a.p0 - src).CompareLength(b.p0 - src) < 0)
return true;
return false;
}
};
WaypointPath VertexPathfinder::ComputeShortPath(const ShortPathRequest& request, CmpPtr cmpObstructionManager) const
{
PROFILE2("ComputeShortPath");
DebugRenderGoal(cmpObstructionManager->GetSimContext(), request.goal);
// Create impassable edges at the max-range boundary, so we can't escape the region
// where we're meant to be searching
fixed rangeXMin = request.x0 - request.range;
fixed rangeXMax = request.x0 + request.range;
fixed rangeZMin = request.z0 - request.range;
fixed rangeZMax = request.z0 + request.range;
// Add domain edges
// (Inside-out square, so edges are in reverse from the usual direction.)
m_Edges.emplace_back(Edge{ CFixedVector2D(rangeXMin, rangeZMin), CFixedVector2D(rangeXMin, rangeZMax) });
m_Edges.emplace_back(Edge{ CFixedVector2D(rangeXMin, rangeZMax), CFixedVector2D(rangeXMax, rangeZMax) });
m_Edges.emplace_back(Edge{ CFixedVector2D(rangeXMax, rangeZMax), CFixedVector2D(rangeXMax, rangeZMin) });
m_Edges.emplace_back(Edge{ CFixedVector2D(rangeXMax, rangeZMin), CFixedVector2D(rangeXMin, rangeZMin) });
// Add the start point to the graph
CFixedVector2D posStart(request.x0, request.z0);
fixed hStart = (posStart - request.goal.NearestPointOnGoal(posStart)).Length();
Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL };
m_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(request.goal.x, request.goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL };
m_Vertexes.push_back(end);
const size_t GOAL_VERTEX_ID = 1;
// Find all the obstruction squares that might affect us
std::vector squares;
size_t staticShapesNb = 0;
ControlGroupMovementObstructionFilter filter(request.avoidMovingUnits, request.group);
cmpObstructionManager->GetStaticObstructionsInRange(filter, rangeXMin - request.clearance, rangeZMin - request.clearance, rangeXMax + request.clearance, rangeZMax + request.clearance, squares);
staticShapesNb = squares.size();
cmpObstructionManager->GetUnitObstructionsInRange(filter, rangeXMin - request.clearance, rangeZMin - request.clearance, rangeXMax + request.clearance, rangeZMax + request.clearance, squares);
// Change array capacities to reduce reallocations
m_Vertexes.reserve(m_Vertexes.size() + squares.size()*4);
m_EdgeSquares.reserve(m_EdgeSquares.size() + squares.size()); // (assume most squares are AA)
entity_pos_t pathfindClearance = request.clearance;
// 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;
if (i >= staticShapesNb)
pathfindClearance = request.clearance - entity_pos_t::FromInt(1)/2;
// Expand the vertexes by the moving unit's collision radius, to find the
// closest we can get to it
CFixedVector2D hd0(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
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;
+ if (aa)
+ {
+ vert.quadInward = QUADRANT_BR;
+ vert.quadOutward = (~vert.quadInward) & 0xF;
+ }
if (vert.p.X >= rangeXMin && vert.p.Y >= rangeZMin && vert.p.X <= rangeXMax && vert.p.Y <= rangeZMax)
m_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;
+ if (aa)
+ {
+ vert.quadInward = QUADRANT_TR;
+ vert.quadOutward = (~vert.quadInward) & 0xF;
+ }
if (vert.p.X >= rangeXMin && vert.p.Y >= rangeZMin && vert.p.X <= rangeXMax && vert.p.Y <= rangeZMax)
m_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;
+ if (aa)
+ {
+ vert.quadInward = QUADRANT_TL;
+ vert.quadOutward = (~vert.quadInward) & 0xF;
+ }
if (vert.p.X >= rangeXMin && vert.p.Y >= rangeZMin && vert.p.X <= rangeXMax && vert.p.Y <= rangeZMax)
m_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;
+ if (aa)
+ {
+ vert.quadInward = QUADRANT_BL;
+ vert.quadOutward = (~vert.quadInward) & 0xF;
+ }
if (vert.p.X >= rangeXMin && vert.p.Y >= rangeZMin && vert.p.X <= rangeXMax && vert.p.Y <= rangeZMax)
m_Vertexes.push_back(vert);
// Add the edges:
CFixedVector2D h0(squares[i].hw + pathfindClearance, squares[i].hh + pathfindClearance);
CFixedVector2D h1(squares[i].hw + pathfindClearance, -(squares[i].hh + pathfindClearance));
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)
m_EdgeSquares.emplace_back(Square{ ev1, ev3 });
else
{
m_Edges.emplace_back(Edge{ ev0, ev1 });
m_Edges.emplace_back(Edge{ ev1, ev2 });
m_Edges.emplace_back(Edge{ ev2, ev3 });
m_Edges.emplace_back(Edge{ ev3, ev0 });
}
}
// Add terrain obstructions
{
u16 i0, j0, i1, j1;
Pathfinding::NearestNavcell(rangeXMin, rangeZMin, i0, j0, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE);
Pathfinding::NearestNavcell(rangeXMax, rangeZMax, i1, j1, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE);
AddTerrainEdges(m_Edges, m_Vertexes, i0, j0, i1, j1, request.passClass, *m_TerrainOnlyGrid);
}
// Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable)
- for (size_t i = 0; i < m_EdgeSquares.size(); ++i)
+ for (size_t i = 2; i < m_EdgeSquares.size(); ++i)
{
// If the start point is inside the square, ignore it
if (start.p.X >= m_EdgeSquares[i].p0.X &&
start.p.Y >= m_EdgeSquares[i].p0.Y &&
start.p.X <= m_EdgeSquares[i].p1.X &&
start.p.Y <= m_EdgeSquares[i].p1.Y)
continue;
// Remove every non-start/goal vertex that is inside an edgeSquare;
// since remove() would be inefficient, just mark it as closed instead.
for (size_t j = 2; j < m_Vertexes.size(); ++j)
if (m_Vertexes[j].p.X >= m_EdgeSquares[i].p0.X &&
m_Vertexes[j].p.Y >= m_EdgeSquares[i].p0.Y &&
m_Vertexes[j].p.X <= m_EdgeSquares[i].p1.X &&
m_Vertexes[j].p.Y <= m_EdgeSquares[i].p1.Y)
m_Vertexes[j].status = Vertex::CLOSED;
}
ENSURE(m_Vertexes.size() < 65536); // We store array indexes as u16.
DebugRenderGraph(cmpObstructionManager->GetSimContext(), m_Vertexes, m_Edges, m_EdgeSquares);
// 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.
VertexPriorityQueue open;
VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h };
open.push(qiStart);
u16 idBest = START_VERTEX_ID;
fixed hBest = start.h;
while (!open.empty())
{
// Move best tile from open to closed
VertexPriorityQueue::Item curr = open.pop();
m_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 by distance in order to check those first that have a high probability of blocking a ray.
// The heuristic based on distance is very rough, especially for squares that are further away;
// we're also only really interested in the closest squares since they are the only ones that block a lot of rays.
// Thus we only do a partial sort; the threshold is just a somewhat reasonable value.
if (m_EdgeSquares.size() > 8)
std::partial_sort(m_EdgeSquares.begin(), m_EdgeSquares.begin() + 8, m_EdgeSquares.end(), SquareSort(m_Vertexes[curr.id].p));
m_EdgesUnaligned.clear();
m_EdgesLeft.clear();
m_EdgesRight.clear();
m_EdgesBottom.clear();
m_EdgesTop.clear();
SplitAAEdges(m_Vertexes[curr.id].p, m_Edges, m_EdgeSquares, m_EdgesUnaligned, m_EdgesLeft, m_EdgesRight, m_EdgesBottom, m_EdgesTop);
// Check the lines to every other vertex
for (size_t n = 0; n < m_Vertexes.size(); ++n)
{
if (m_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 = request.goal.NearestPointOnGoal(m_Vertexes[curr.id].p);
// 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);
+ npos.X = clamp(npos.X, rangeXMin + EDGE_EXPAND_DELTA, rangeXMax - EDGE_EXPAND_DELTA);
+ npos.Y = clamp(npos.Y, rangeZMin + EDGE_EXPAND_DELTA, rangeZMax - EDGE_EXPAND_DELTA);
}
else
npos = m_Vertexes[n].p;
// Work out which quadrant(s) we're approaching the new vertex from
u8 quad = 0;
if (m_Vertexes[curr.id].p.X <= npos.X && m_Vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL;
if (m_Vertexes[curr.id].p.X >= npos.X && m_Vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR;
if (m_Vertexes[curr.id].p.X <= npos.X && m_Vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL;
if (m_Vertexes[curr.id].p.X >= npos.X && m_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 (!(m_Vertexes[curr.id].quadOutward & quad))
+ if (!(m_Vertexes[curr.id].quadOutward & quad) && curr.id != START_VERTEX_ID)
{
// 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(m_Vertexes[curr.id].p, npos, m_EdgesLeft) &&
CheckVisibilityRight(m_Vertexes[curr.id].p, npos, m_EdgesRight) &&
CheckVisibilityBottom(m_Vertexes[curr.id].p, npos, m_EdgesBottom) &&
CheckVisibilityTop(m_Vertexes[curr.id].p, npos, m_EdgesTop) &&
CheckVisibility(m_Vertexes[curr.id].p, npos, m_EdgesUnaligned);
// Render the edges that we examine.
DebugRenderEdges(cmpObstructionManager->GetSimContext(), visible, m_Vertexes[curr.id].p, npos);
if (visible)
{
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 (m_Vertexes[n].status == Vertex::UNEXPLORED)
{
// Add it to the open list:
m_Vertexes[n].status = Vertex::OPEN;
m_Vertexes[n].g = g;
m_Vertexes[n].h = request.goal.DistanceToPoint(npos);
m_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 (m_Vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8)))
- m_Vertexes[n].quadOutward = ((m_Vertexes[n].quadInward) & quad) & 0xF;
-
if (n == GOAL_VERTEX_ID)
m_Vertexes[n].p = npos; // remember the new best goal position
VertexPriorityQueue::Item t = { (u16)n, g + m_Vertexes[n].h, m_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 (m_Vertexes[n].h < hBest)
{
idBest = (u16)n;
hBest = m_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 >= m_Vertexes[n].g)
continue;
// Otherwise, we have a better path, so replace the old one with the new cost/parent
fixed gprev = m_Vertexes[n].g;
m_Vertexes[n].g = g;
m_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 (m_Vertexes[n].quadInward)
- m_Vertexes[n].quadOutward = ((m_Vertexes[n].quadInward) & quad) & 0xF;
-
if (n == GOAL_VERTEX_ID)
m_Vertexes[n].p = npos; // remember the new best goal position
open.promote((u16)n, gprev + m_Vertexes[n].h, g + m_Vertexes[n].h, m_Vertexes[n].h);
}
}
}
}
// Reconstruct the path (in reverse)
WaypointPath path;
for (u16 id = idBest; id != START_VERTEX_ID; id = m_Vertexes[id].pred)
path.m_Waypoints.emplace_back(Waypoint{ m_Vertexes[id].p.X, m_Vertexes[id].p.Y });
m_Edges.clear();
m_EdgeSquares.clear();
m_Vertexes.clear();
m_EdgesUnaligned.clear();
m_EdgesLeft.clear();
m_EdgesRight.clear();
m_EdgesBottom.clear();
m_EdgesTop.clear();
return path;
}
void VertexPathfinder::DebugRenderGoal(const CSimContext& simContext, const PathGoal& goal) const
{
if (!m_DebugOverlay)
return;
m_DebugOverlayShortPathLines.clear();
// Render the goal shape
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1);
switch (goal.type)
{
case PathGoal::POINT:
{
SimRender::ConstructCircleOnGround(simContext, goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true);
break;
}
case PathGoal::CIRCLE:
case PathGoal::INVERTED_CIRCLE:
{
SimRender::ConstructCircleOnGround(simContext, goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true);
break;
}
case PathGoal::SQUARE:
case PathGoal::INVERTED_SQUARE:
{
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);
break;
}
}
}
void VertexPathfinder::DebugRenderGraph(const CSimContext& simContext, const std::vector& vertexes, const std::vector& edges, const std::vector& edgeSquares) const
{
if (!m_DebugOverlay)
return;
#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
for (size_t i = 0; i < vertexes.size(); ++i)
{
m_DebugOverlayShortPathLines.emplace_back();
m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1);
float x = vertexes[i].p.X.ToFloat();
float z = vertexes[i].p.Y.ToFloat();
float a0 = 0, a1 = 0;
// Get arc start/end angles depending on quadrant (if any)
if (vertexes[i].quadInward == QUADRANT_BL) { a0 = -0.25f; a1 = 0.50f; }
else if (vertexes[i].quadInward == QUADRANT_TR) { a0 = 0.25f; a1 = 1.00f; }
else if (vertexes[i].quadInward == QUADRANT_TL) { a0 = -0.50f; a1 = 0.25f; }
else if (vertexes[i].quadInward == QUADRANT_BR) { a0 = 0.00f; a1 = 0.75f; }
if (a0 == a1)
SimRender::ConstructCircleOnGround(simContext, x, z, 0.5f,
m_DebugOverlayShortPathLines.back(), true);
else
SimRender::ConstructClosedArcOnGround(simContext, x, z, 0.5f,
a0 * ((float)M_PI*2.0f), a1 * ((float)M_PI*2.0f),
m_DebugOverlayShortPathLines.back(), true);
}
// Render the edges
for (size_t i = 0; i < edges.size(); ++i)
{
m_DebugOverlayShortPathLines.emplace_back();
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
std::vector xz;
PUSH_POINT(edges[i].p0);
PUSH_POINT(edges[i].p1);
// Add an arrowhead to indicate the direction
CFixedVector2D d = edges[i].p1 - edges[i].p0;
d.Normalize(fixed::FromInt(1)/8);
CFixedVector2D p2 = edges[i].p1 - d*2;
CFixedVector2D p3 = p2 + d.Perpendicular();
CFixedVector2D p4 = p2 - d.Perpendicular();
PUSH_POINT(p3);
PUSH_POINT(p4);
PUSH_POINT(edges[i].p1);
SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), true);
}
#undef PUSH_POINT
// Render the axis-aligned squares
for (size_t i = 0; i < edgeSquares.size(); ++i)
{
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
std::vector xz;
Square s = edgeSquares[i];
xz.push_back(s.p0.X.ToFloat());
xz.push_back(s.p0.Y.ToFloat());
xz.push_back(s.p0.X.ToFloat());
xz.push_back(s.p1.Y.ToFloat());
xz.push_back(s.p1.X.ToFloat());
xz.push_back(s.p1.Y.ToFloat());
xz.push_back(s.p1.X.ToFloat());
xz.push_back(s.p0.Y.ToFloat());
xz.push_back(s.p0.X.ToFloat());
xz.push_back(s.p0.Y.ToFloat());
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
{
if (!m_DebugOverlay)
return;
// Disabled by default.
/*
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.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5);
std::vector xz;
xz.push_back(curr.X.ToFloat());
xz.push_back(curr.Y.ToFloat());
xz.push_back(npos.X.ToFloat());
xz.push_back(npos.Y.ToFloat());
SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false);
SimRender::ConstructLineOnGround(simContext, xz, m_DebugOverlayShortPathLines.back(), false);
*/
}
void VertexPathfinder::RenderSubmit(SceneCollector& collector)
{
if (!m_DebugOverlay)
return;
for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i)
collector.Submit(&m_DebugOverlayShortPathLines[i]);
}