ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckBuildingPlacement(const IObstructionTestFilter& filter,
entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w,
entity_pos_t h, entity_id_t id, pass_class_t passClass, bool onlyCenterPoint)
{
// Check unit obstruction
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
if (!cmpObstructionManager)
return ICmpObstruction::FOUNDATION_CHECK_FAIL_ERROR;
if (cmpObstructionManager->TestStaticShape(filter, x, z, a, w, h, NULL))
return ICmpObstruction::FOUNDATION_CHECK_FAIL_OBSTRUCTS_FOUNDATION;
// Test against terrain:
UpdateGrid();
ICmpObstructionManager::ObstructionSquare square;
CmpPtr<ICmpObstruction> cmpObstruction(GetSimContext(), id);
if (!cmpObstruction || !cmpObstruction->GetObstructionSquare(square))
return ICmpObstruction::FOUNDATION_CHECK_FAIL_NO_OBSTRUCTION;
if (onlyCenterPoint)
{
u16 i, j;
NearestTile(x, z, i, j);
if (IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass))
return ICmpObstruction::FOUNDATION_CHECK_SUCCESS;
return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS;
}
// Expand bounds by 1/sqrt(2) tile (multiply by TERRAIN_TILE_SIZE since we want world coordinates)
entity_pos_t expand = entity_pos_t::FromInt(2).Sqrt().Multiply(entity_pos_t::FromInt(TERRAIN_TILE_SIZE / 2));
CFixedVector2D halfSize(square.hw + expand, square.hh + expand);
CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(square.u, square.v, halfSize);
u16 i0, j0, i1, j1;
NearestTile(square.x - halfBound.X, square.z - halfBound.Y, i0, j0);
NearestTile(square.x + halfBound.X, square.z + halfBound.Y, i1, j1);
for (u16 j = j0; j <= j1; ++j)
{
for (u16 i = i0; i <= i1; ++i)
{
entity_pos_t x, z;
TileCenter(i, j, x, z);
if (Geometry::PointIsInSquare(CFixedVector2D(x - square.x, z - square.z), square.u, square.v, halfSize)
&& !IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass))
{
return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS;
}
}
}
return ICmpObstruction::FOUNDATION_CHECK_SUCCESS;
}
//-------------------------------------------------------------------------
//
// Convert tile center plus pixel offset to lat/lon. Offset increases right and down
//
LonLat GoogleMapSource::tileCenterToLonLat( const int tileSize, const MapTileCoordinate& tile, const double offsetX, const double offsetY )
//-------------------------------------------------------------------------
{
double meterX, meterY;
TileCenter( getTileSize( ), tile.getX( ), tile.getY( ), tile.getMagnification( ), meterX, meterY );
double pixelX, pixelY;
MetersToPixels( meterX, meterY, tile.getMagnification( ), pixelX, pixelY );
pixelX += offsetX;
pixelY -= offsetY; // google maps y coords increase up
PixelsToMeters( pixelX, pixelY, tile.getMagnification( ), meterX, meterY );
double lon, lat;
MetersToLonLat( meterX, meterY, lon, lat );
return LonLat( lon, lat );
}
void CCmpPathfinder::UpdateGrid()
{
CmpPtr<ICmpTerrain> cmpTerrain(GetSimContext(), SYSTEM_ENTITY);
if (!cmpTerrain)
return; // error
// If the terrain was resized then delete the old grid data
if (m_Grid && m_MapSize != cmpTerrain->GetTilesPerSide())
{
SAFE_DELETE(m_Grid);
SAFE_DELETE(m_ObstructionGrid);
m_TerrainDirty = true;
}
// Initialise the terrain data when first needed
if (!m_Grid)
{
m_MapSize = cmpTerrain->GetTilesPerSide();
m_Grid = new Grid<TerrainTile>(m_MapSize, m_MapSize);
m_ObstructionGrid = new Grid<u8>(m_MapSize, m_MapSize);
}
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
bool obstructionsDirty = cmpObstructionManager->Rasterise(*m_ObstructionGrid);
if (obstructionsDirty && !m_TerrainDirty)
{
PROFILE("UpdateGrid obstructions");
// Obstructions changed - we need to recompute passability
// Since terrain hasn't changed we only need to update the obstruction bits
// and can skip the rest of the data
// TODO: if ObstructionManager::SetPassabilityCircular was called at runtime
// (which should probably never happen, but that's not guaranteed),
// then TILE_OUTOFBOUNDS will change and we can't use this fast path, but
// currently it'll just set obstructionsDirty and we won't notice
for (u16 j = 0; j < m_MapSize; ++j)
{
for (u16 i = 0; i < m_MapSize; ++i)
{
TerrainTile& t = m_Grid->get(i, j);
u8 obstruct = m_ObstructionGrid->get(i, j);
if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING)
t |= 1;
else
t &= (TerrainTile)~1;
if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_FOUNDATION)
t |= 2;
else
t &= (TerrainTile)~2;
}
}
++m_Grid->m_DirtyID;
}
else if (obstructionsDirty || m_TerrainDirty)
{
PROFILE("UpdateGrid full");
// Obstructions or terrain changed - we need to recompute passability
// TODO: only bother recomputing the region that has actually changed
CmpPtr<ICmpWaterManager> cmpWaterManager(GetSimContext(), SYSTEM_ENTITY);
// TOOD: these bits should come from ICmpTerrain
CTerrain& terrain = GetSimContext().GetTerrain();
// avoid integer overflow in intermediate calculation
const u16 shoreMax = 32767;
// First pass - find underwater tiles
Grid<bool> waterGrid(m_MapSize, m_MapSize);
for (u16 j = 0; j < m_MapSize; ++j)
{
for (u16 i = 0; i < m_MapSize; ++i)
{
fixed x, z;
TileCenter(i, j, x, z);
bool underWater = cmpWaterManager && (cmpWaterManager->GetWaterLevel(x, z) > terrain.GetExactGroundLevelFixed(x, z));
waterGrid.set(i, j, underWater);
}
}
// Second pass - find shore tiles
Grid<u16> shoreGrid(m_MapSize, m_MapSize);
for (u16 j = 0; j < m_MapSize; ++j)
{
for (u16 i = 0; i < m_MapSize; ++i)
{
// Find a land tile
if (!waterGrid.get(i, j))
{
if ((i > 0 && waterGrid.get(i-1, j)) || (i > 0 && j < m_MapSize-1 && waterGrid.get(i-1, j+1)) || (i > 0 && j > 0 && waterGrid.get(i-1, j-1))
|| (i < m_MapSize-1 && waterGrid.get(i+1, j)) || (i < m_MapSize-1 && j < m_MapSize-1 && waterGrid.get(i+1, j+1)) || (i < m_MapSize-1 && j > 0 && waterGrid.get(i+1, j-1))
|| (j > 0 && waterGrid.get(i, j-1)) || (j < m_MapSize-1 && waterGrid.get(i, j+1))
)
{ // If it's bordered by water, it's a shore tile
shoreGrid.set(i, j, 0);
}
else
{
shoreGrid.set(i, j, shoreMax);
}
}
}
}
// Expand influences on land to find shore distance
for (u16 y = 0; y < m_MapSize; ++y)
{
u16 min = shoreMax;
for (u16 x = 0; x < m_MapSize; ++x)
{
if (!waterGrid.get(x, y))
{
u16 g = shoreGrid.get(x, y);
if (g > min)
shoreGrid.set(x, y, min);
else if (g < min)
min = g;
++min;
}
}
for (u16 x = m_MapSize; x > 0; --x)
{
if (!waterGrid.get(x-1, y))
{
u16 g = shoreGrid.get(x-1, y);
if (g > min)
shoreGrid.set(x-1, y, min);
else if (g < min)
min = g;
++min;
}
}
}
for (u16 x = 0; x < m_MapSize; ++x)
{
u16 min = shoreMax;
for (u16 y = 0; y < m_MapSize; ++y)
{
if (!waterGrid.get(x, y))
{
u16 g = shoreGrid.get(x, y);
if (g > min)
shoreGrid.set(x, y, min);
else if (g < min)
min = g;
++min;
}
}
for (u16 y = m_MapSize; y > 0; --y)
{
if (!waterGrid.get(x, y-1))
{
u16 g = shoreGrid.get(x, y-1);
if (g > min)
shoreGrid.set(x, y-1, min);
else if (g < min)
min = g;
++min;
}
}
}
// Apply passability classes to terrain
for (u16 j = 0; j < m_MapSize; ++j)
{
for (u16 i = 0; i < m_MapSize; ++i)
{
fixed x, z;
TileCenter(i, j, x, z);
TerrainTile t = 0;
u8 obstruct = m_ObstructionGrid->get(i, j);
fixed height = terrain.GetExactGroundLevelFixed(x, z);
fixed water;
if (cmpWaterManager)
water = cmpWaterManager->GetWaterLevel(x, z);
fixed depth = water - height;
fixed slope = terrain.GetSlopeFixed(i, j);
fixed shoredist = fixed::FromInt(shoreGrid.get(i, j));
if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING)
t |= 1;
if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_FOUNDATION)
t |= 2;
if (obstruct & ICmpObstructionManager::TILE_OUTOFBOUNDS)
{
// If out of bounds, nobody is allowed to pass
for (size_t n = 0; n < m_PassClasses.size(); ++n)
t |= m_PassClasses[n].m_Mask;
}
else
{
for (size_t n = 0; n < m_PassClasses.size(); ++n)
{
if (!m_PassClasses[n].IsPassable(depth, slope, shoredist))
t |= m_PassClasses[n].m_Mask;
}
}
std::string moveClass = terrain.GetMovementClass(i, j);
if (m_TerrainCostClassTags.find(moveClass) != m_TerrainCostClassTags.end())
t |= COST_CLASS_MASK(m_TerrainCostClassTags[moveClass]);
m_Grid->set(i, j, t);
}
}
m_TerrainDirty = false;
++m_Grid->m_DirtyID;
}
}
void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& path)
{
UpdateGrid();
PROFILE("ComputePath");
PathfinderState state = { 0 };
// Convert the start/end coordinates to tile indexes
u16 i0, j0;
NearestTile(x0, z0, i0, j0);
NearestTile(goal.x, goal.z, state.iGoal, state.jGoal);
// If we're already at the goal tile, then move directly to the exact goal coordinates
if (AtGoal(i0, j0, goal))
{
Waypoint w = { goal.x, goal.z };
path.m_Waypoints.push_back(w);
return;
}
// If the target is a circle, we want to aim for the edge of it (so e.g. if we're inside
// a large circle then the heuristics will aim us directly outwards);
// otherwise just aim at the center point. (We'll never try moving outwards to a square shape.)
if (goal.type == Goal::CIRCLE)
state.rGoal = (goal.hw / (int)CELL_SIZE).ToInt_RoundToZero();
else
state.rGoal = 0;
state.passClass = passClass;
state.moveCosts = m_MoveCosts.at(costClass);
state.steps = 0;
state.tiles = new PathfindTileGrid(m_MapSize, m_MapSize);
state.terrain = m_Grid;
state.iBest = i0;
state.jBest = j0;
state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal, state.rGoal);
PriorityQueue::Item start = { std::make_pair(i0, j0), 0 };
state.open.push(start);
state.tiles->get(i0, j0).SetStatusOpen();
state.tiles->get(i0, j0).SetPred(i0, j0, i0, j0);
state.tiles->get(i0, j0).cost = 0;
// To prevent units getting very stuck, if they start on an impassable tile
// surrounded entirely by impassable tiles, we ignore the impassability
state.ignoreImpassable = !IS_PASSABLE(state.terrain->get(i0, j0), state.passClass);
while (1)
{
++state.steps;
// Hack to avoid spending ages computing giant paths, particularly when
// the destination is unreachable
if (state.steps > 40000)
break;
// If we ran out of tiles to examine, give up
if (state.open.empty())
break;
#if PATHFIND_STATS
state.sumOpenSize += state.open.size();
#endif
// Move best tile from open to closed
PriorityQueue::Item curr = state.open.pop();
u16 i = curr.id.first;
u16 j = curr.id.second;
state.tiles->get(i, j).SetStatusClosed();
// If we've reached the destination, stop
if (AtGoal(i, j, goal))
{
state.iBest = i;
state.jBest = j;
state.hBest = 0;
break;
}
// As soon as we find an escape route from the impassable terrain,
// take it and forbid any further use of impassable tiles
if (state.ignoreImpassable)
{
if (i > 0 && IS_PASSABLE(state.terrain->get(i-1, j), state.passClass))
state.ignoreImpassable = false;
else if (i < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i+1, j), state.passClass))
state.ignoreImpassable = false;
else if (j > 0 && IS_PASSABLE(state.terrain->get(i, j-1), state.passClass))
state.ignoreImpassable = false;
else if (j < m_MapSize-1 && IS_PASSABLE(state.terrain->get(i, j+1), state.passClass))
state.ignoreImpassable = false;
}
u32 g = state.tiles->get(i, j).cost;
if (i > 0)
ProcessNeighbour(i, j, i-1, j, g, state);
if (i < m_MapSize-1)
ProcessNeighbour(i, j, i+1, j, g, state);
if (j > 0)
ProcessNeighbour(i, j, i, j-1, g, state);
if (j < m_MapSize-1)
ProcessNeighbour(i, j, i, j+1, g, state);
}
// Reconstruct the path (in reverse)
u16 ip = state.iBest, jp = state.jBest;
while (ip != i0 || jp != j0)
{
PathfindTile& n = state.tiles->get(ip, jp);
entity_pos_t x, z;
TileCenter(ip, jp, x, z);
Waypoint w = { x, z };
path.m_Waypoints.push_back(w);
// Follow the predecessor link
ip = n.GetPredI(ip);
jp = n.GetPredJ(jp);
}
// Save this grid for debug display
delete m_DebugGrid;
m_DebugGrid = state.tiles;
m_DebugSteps = state.steps;
#if PATHFIND_STATS
printf("PATHFINDER: steps=%d avgo=%d proc=%d impc=%d impo=%d addo=%d\n", state.steps, state.sumOpenSize/state.steps, state.numProcessed, state.numImproveClosed, state.numImproveOpen, state.numAddToOpen);
#endif
}
void CCmpPathfinder::UpdateGrid()
{
// If the terrain was resized then delete the old grid data
if (m_Grid && m_MapSize != GetSimContext().GetTerrain().GetTilesPerSide())
{
SAFE_DELETE(m_Grid);
SAFE_DELETE(m_ObstructionGrid);
m_TerrainDirty = true;
}
// Initialise the terrain data when first needed
if (!m_Grid)
{
// TOOD: these bits should come from ICmpTerrain
ssize_t size = GetSimContext().GetTerrain().GetTilesPerSide();
ENSURE(size >= 1 && size <= 0xffff); // must fit in 16 bits
m_MapSize = size;
m_Grid = new Grid<TerrainTile>(m_MapSize, m_MapSize);
m_ObstructionGrid = new Grid<u8>(m_MapSize, m_MapSize);
}
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
bool obstructionsDirty = cmpObstructionManager->Rasterise(*m_ObstructionGrid);
if (obstructionsDirty && !m_TerrainDirty)
{
PROFILE("UpdateGrid obstructions");
// Obstructions changed - we need to recompute passability
// Since terrain hasn't changed we only need to update the obstruction bits
// and can skip the rest of the data
// TODO: if ObstructionManager::SetPassabilityCircular was called at runtime
// (which should probably never happen, but that's not guaranteed),
// then TILE_OUTOFBOUNDS will change and we can't use this fast path, but
// currently it'll just set obstructionsDirty and we won't notice
for (u16 j = 0; j < m_MapSize; ++j)
{
for (u16 i = 0; i < m_MapSize; ++i)
{
TerrainTile& t = m_Grid->get(i, j);
u8 obstruct = m_ObstructionGrid->get(i, j);
if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING)
t |= 1;
else
t &= ~1;
if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_FOUNDATION)
t |= 2;
else
t &= ~2;
}
}
++m_Grid->m_DirtyID;
}
else if (obstructionsDirty || m_TerrainDirty)
{
PROFILE("UpdateGrid full");
// Obstructions or terrain changed - we need to recompute passability
// TODO: only bother recomputing the region that has actually changed
CmpPtr<ICmpWaterManager> cmpWaterMan(GetSimContext(), SYSTEM_ENTITY);
CTerrain& terrain = GetSimContext().GetTerrain();
for (u16 j = 0; j < m_MapSize; ++j)
{
for (u16 i = 0; i < m_MapSize; ++i)
{
fixed x, z;
TileCenter(i, j, x, z);
TerrainTile t = 0;
u8 obstruct = m_ObstructionGrid->get(i, j);
fixed height = terrain.GetVertexGroundLevelFixed(i, j); // TODO: should use tile centre
fixed water;
if (!cmpWaterMan.null())
water = cmpWaterMan->GetWaterLevel(x, z);
fixed depth = water - height;
fixed slope = terrain.GetSlopeFixed(i, j);
if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING)
t |= 1;
if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_FOUNDATION)
t |= 2;
if (obstruct & ICmpObstructionManager::TILE_OUTOFBOUNDS)
{
// If out of bounds, nobody is allowed to pass
for (size_t n = 0; n < m_PassClasses.size(); ++n)
t |= m_PassClasses[n].m_Mask;
}
else
{
for (size_t n = 0; n < m_PassClasses.size(); ++n)
{
if (!m_PassClasses[n].IsPassable(depth, slope))
t |= m_PassClasses[n].m_Mask;
}
}
std::string moveClass = terrain.GetMovementClass(i, j);
if (m_TerrainCostClassTags.find(moveClass) != m_TerrainCostClassTags.end())
t |= COST_CLASS_MASK(m_TerrainCostClassTags[moveClass]);
m_Grid->set(i, j, t);
}
}
m_TerrainDirty = false;
++m_Grid->m_DirtyID;
}
}