diff --git a/binaries/data/mods/public/gui/session/session.xml b/binaries/data/mods/public/gui/session/session.xml index ad84a47307..d9ee7da96c 100644 --- a/binaries/data/mods/public/gui/session/session.xml +++ b/binaries/data/mods/public/gui/session/session.xml @@ -51,7 +51,7 @@ /> - diff --git a/binaries/data/mods/public/maps/scenarios/Pathfinding_demo.pmp b/binaries/data/mods/public/maps/scenarios/Pathfinding_demo.pmp index d7969a8cde..bba1a8f3ee 100644 --- a/binaries/data/mods/public/maps/scenarios/Pathfinding_demo.pmp +++ b/binaries/data/mods/public/maps/scenarios/Pathfinding_demo.pmp @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c06770c5cc0e8ac8d106df6414418f5aa9ba82b22dc0693e4d68a469e98ce17a +oid sha256:a33507c626d7f32c7878c4955c0083d80c6acb4d761e81698b938813e3a5942d size 207975 diff --git a/binaries/data/mods/public/maps/scenarios/Pathfinding_demo.xml b/binaries/data/mods/public/maps/scenarios/Pathfinding_demo.xml index 7ff57b3943..f24083131e 100644 --- a/binaries/data/mods/public/maps/scenarios/Pathfinding_demo.xml +++ b/binaries/data/mods/public/maps/scenarios/Pathfinding_demo.xml @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:43ed58f143416bed804facb492dd2ca13551a844ca8308579d1d95851c62b6c4 -size 19955 +oid sha256:921bb388b025a4016fe39968c9fdd85682e0a3c0e8450d9c22e57c69e4523974 +size 20815 diff --git a/binaries/data/mods/public/simulation/components/BuildRestrictions.js b/binaries/data/mods/public/simulation/components/BuildRestrictions.js index b13d6bb80d..a3fc586909 100644 --- a/binaries/data/mods/public/simulation/components/BuildRestrictions.js +++ b/binaries/data/mods/public/simulation/components/BuildRestrictions.js @@ -117,11 +117,11 @@ BuildRestrictions.prototype.CheckPlacement = function() break; case "land-shore": - // 'default' is everywhere a normal unit can go - // So on passable land, and not too deep in the water - passClassName = "default"; + // 'default-terrain-only' is everywhere a normal unit can go, ignoring + // obstructions (i.e. on passable land, and not too deep in the water) + passClassName = "default-terrain-only"; break; - + case "land": default: passClassName = "building-land"; @@ -130,8 +130,8 @@ BuildRestrictions.prototype.CheckPlacement = function() var cmpObstruction = Engine.QueryInterface(this.entity, IID_Obstruction); if (!cmpObstruction) return result; // Fail - - + + if (this.template.Category == "Wall") { // for walls, only test the center point diff --git a/binaries/data/mods/public/simulation/components/GuiInterface.js b/binaries/data/mods/public/simulation/components/GuiInterface.js index e1f6fa2bf0..2aa6e9a0b2 100644 --- a/binaries/data/mods/public/simulation/components/GuiInterface.js +++ b/binaries/data/mods/public/simulation/components/GuiInterface.js @@ -1777,6 +1777,12 @@ GuiInterface.prototype.SetPathfinderDebugOverlay = function(player, enabled) cmpPathfinder.SetDebugOverlay(enabled); }; +GuiInterface.prototype.SetPathfinderHierDebugOverlay = function(player, enabled) +{ + var cmpPathfinder = Engine.QueryInterface(SYSTEM_ENTITY, IID_Pathfinder); + cmpPathfinder.SetHierDebugOverlay(enabled); +}; + GuiInterface.prototype.SetObstructionDebugOverlay = function(player, enabled) { var cmpObstructionManager = Engine.QueryInterface(SYSTEM_ENTITY, IID_ObstructionManager); @@ -1899,6 +1905,7 @@ var exposedFunctions = { "IsMapRevealed": 1, "SetPathfinderDebugOverlay": 1, + "SetPathfinderHierDebugOverlay": 1, "SetObstructionDebugOverlay": 1, "SetMotionDebugOverlay": 1, "SetRangeDebugOverlay": 1, diff --git a/binaries/data/mods/public/simulation/data/pathfinder.rnc b/binaries/data/mods/public/simulation/data/pathfinder.rnc index 50f40f95a3..58f0a24541 100644 --- a/binaries/data/mods/public/simulation/data/pathfinder.rnc +++ b/binaries/data/mods/public/simulation/data/pathfinder.rnc @@ -12,19 +12,8 @@ element Pathfinder { element MaxWaterDepth { xsd:decimal }? & element MaxTerrainSlope { xsd:decimal }? & element MinShoreDistance { xsd:decimal }? & - element MaxShoreDistance { xsd:decimal }? - }+ - } & - element MovementClasses { - element * { - attribute Speed { xsd:decimal } & - attribute Cost { xsd:decimal } & - element UnitClasses { - element * { - attribute Speed { xsd:decimal } & - attribute Cost { xsd:decimal } - }* - }? + element MaxShoreDistance { xsd:decimal }? & + element Clearance { xsd:decimal }? }+ } } diff --git a/binaries/data/mods/public/simulation/data/pathfinder.rng b/binaries/data/mods/public/simulation/data/pathfinder.rng index 693f9d93c8..a654335fb1 100644 --- a/binaries/data/mods/public/simulation/data/pathfinder.rng +++ b/binaries/data/mods/public/simulation/data/pathfinder.rng @@ -41,36 +41,9 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + diff --git a/binaries/data/mods/public/simulation/data/pathfinder.xml b/binaries/data/mods/public/simulation/data/pathfinder.xml index 47eb0eba6b..35e9797330 100644 --- a/binaries/data/mods/public/simulation/data/pathfinder.xml +++ b/binaries/data/mods/public/simulation/data/pathfinder.xml @@ -6,46 +6,64 @@ - + + + 2 1.0 + 1.0 + + 2 + 1.0 + 0.0 + + + 2 + 1.0 + + + 2 + 1.0 + 4.0 + 1 + 12.0 + + 1 + 4.0 + + 0 - 1.0 + 4.0 1.0 - 2.0 + 8.0 1.25 + + + 2 + 1.0 + + - - - - - - - - - - diff --git a/binaries/data/mods/public/simulation/templates/other/fence_long.xml b/binaries/data/mods/public/simulation/templates/other/fence_long.xml index df2b6dd5b3..f92fa9aa6f 100644 --- a/binaries/data/mods/public/simulation/templates/other/fence_long.xml +++ b/binaries/data/mods/public/simulation/templates/other/fence_long.xml @@ -33,7 +33,7 @@ - + 6.0 diff --git a/binaries/data/mods/public/simulation/templates/other/fence_short.xml b/binaries/data/mods/public/simulation/templates/other/fence_short.xml index d8f21594a9..b8b4c23a2d 100644 --- a/binaries/data/mods/public/simulation/templates/other/fence_short.xml +++ b/binaries/data/mods/public/simulation/templates/other/fence_short.xml @@ -33,7 +33,7 @@ - + 6.0 diff --git a/binaries/data/mods/public/simulation/templates/other/fence_stone.xml b/binaries/data/mods/public/simulation/templates/other/fence_stone.xml index 37cece3d7f..6303e65cd2 100644 --- a/binaries/data/mods/public/simulation/templates/other/fence_stone.xml +++ b/binaries/data/mods/public/simulation/templates/other/fence_stone.xml @@ -30,7 +30,7 @@ 0 - + 6.0 diff --git a/binaries/data/mods/public/simulation/templates/template_formation.xml b/binaries/data/mods/public/simulation/templates/template_formation.xml index d3077711b8..c17f0e7841 100644 --- a/binaries/data/mods/public/simulation/templates/template_formation.xml +++ b/binaries/data/mods/public/simulation/templates/template_formation.xml @@ -38,8 +38,7 @@ true 1.0 - default - default + siege-large 1.0 diff --git a/binaries/data/mods/public/simulation/templates/template_structure.xml b/binaries/data/mods/public/simulation/templates/template_structure.xml index 9b07eefd15..cd15268560 100644 --- a/binaries/data/mods/public/simulation/templates/template_structure.xml +++ b/binaries/data/mods/public/simulation/templates/template_structure.xml @@ -77,7 +77,6 @@ square round - default default diff --git a/binaries/data/mods/public/simulation/templates/template_unit.xml b/binaries/data/mods/public/simulation/templates/template_unit.xml index c43c4c6e43..761c00ad9b 100644 --- a/binaries/data/mods/public/simulation/templates/template_unit.xml +++ b/binaries/data/mods/public/simulation/templates/template_unit.xml @@ -39,13 +39,6 @@ Unit ConquestCritical formations/null - formations/box - formations/column_closed - formations/line_closed - formations/column_open - formations/line_open - formations/flank - formations/battle_line @@ -113,7 +106,6 @@ 0.2 default - default false diff --git a/binaries/data/mods/public/simulation/templates/template_unit_champion_elephant.xml b/binaries/data/mods/public/simulation/templates/template_unit_champion_elephant.xml index 6643ba412a..72c0927bdf 100644 --- a/binaries/data/mods/public/simulation/templates/template_unit_champion_elephant.xml +++ b/binaries/data/mods/public/simulation/templates/template_unit_champion_elephant.xml @@ -63,6 +63,7 @@ 6.5 + siege-large 8.5 14.0 diff --git a/binaries/data/mods/public/simulation/templates/template_unit_infantry.xml b/binaries/data/mods/public/simulation/templates/template_unit_infantry.xml index 70d9ec6643..b48d36d277 100644 --- a/binaries/data/mods/public/simulation/templates/template_unit_infantry.xml +++ b/binaries/data/mods/public/simulation/templates/template_unit_infantry.xml @@ -126,7 +126,6 @@ 18.75 - infantry 80 diff --git a/binaries/data/mods/public/simulation/templates/template_unit_mechanical_ship_fishing.xml b/binaries/data/mods/public/simulation/templates/template_unit_mechanical_ship_fishing.xml index 9be57ceed8..69da52423e 100644 --- a/binaries/data/mods/public/simulation/templates/template_unit_mechanical_ship_fishing.xml +++ b/binaries/data/mods/public/simulation/templates/template_unit_mechanical_ship_fishing.xml @@ -55,6 +55,7 @@ false + ship-small 10 diff --git a/binaries/data/mods/public/simulation/templates/template_unit_mechanical_siege_ram.xml b/binaries/data/mods/public/simulation/templates/template_unit_mechanical_siege_ram.xml index b9829eaa57..45a2318638 100644 --- a/binaries/data/mods/public/simulation/templates/template_unit_mechanical_siege_ram.xml +++ b/binaries/data/mods/public/simulation/templates/template_unit_mechanical_siege_ram.xml @@ -60,6 +60,7 @@ 0.5 + siege-large 8 11.0 diff --git a/source/graphics/Terrain.cpp b/source/graphics/Terrain.cpp index b9b8180752..d82866bacf 100644 --- a/source/graphics/Terrain.cpp +++ b/source/graphics/Terrain.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2013 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -36,6 +36,9 @@ #include "maths/FixedVector3D.h" #include "maths/MathUtil.h" #include "ps/CLogger.h" +#include "simulation2/helpers/Pathfinding.h" + +const fixed Pathfinding::NAVCELL_SIZE = fixed::FromInt(TERRAIN_TILE_SIZE) / Pathfinding::NAVCELLS_PER_TILE; /////////////////////////////////////////////////////////////////////////////// // CTerrain constructor @@ -339,6 +342,58 @@ fixed CTerrain::GetSlopeFixed(ssize_t i, ssize_t j) const return fixed::FromInt(delta / TERRAIN_TILE_SIZE) / (int)HEIGHT_UNITS_PER_METRE; } +fixed CTerrain::GetExactSlopeFixed(fixed x, fixed z) const +{ + // Clamp to size-2 so we can use the tiles (xi,zi)-(xi+1,zi+1) + const ssize_t xi = clamp((ssize_t)(x / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), (ssize_t)0, m_MapSize-2); + const ssize_t zi = clamp((ssize_t)(z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), (ssize_t)0, m_MapSize-2); + + const fixed one = fixed::FromInt(1); + + const fixed xf = clamp((x / (int)TERRAIN_TILE_SIZE) - fixed::FromInt(xi), fixed::Zero(), one); + const fixed zf = clamp((z / (int)TERRAIN_TILE_SIZE) - fixed::FromInt(zi), fixed::Zero(), one); + + u16 h00 = m_Heightmap[zi*m_MapSize + xi]; + u16 h01 = m_Heightmap[(zi+1)*m_MapSize + xi]; + u16 h10 = m_Heightmap[zi*m_MapSize + (xi+1)]; + u16 h11 = m_Heightmap[(zi+1)*m_MapSize + (xi+1)]; + + u16 delta; + if (GetTriangulationDir(xi, zi)) + { + if (xf + zf <= one) + { + // Lower-left triangle (don't use h11) + delta = std::max(std::max(h00, h01), h10) - + std::min(std::min(h00, h01), h10); + } + else + { + // Upper-right triangle (don't use h00) + delta = std::max(std::max(h01, h10), h11) - + std::min(std::min(h01, h10), h11); + } + } + else + { + if (xf <= zf) + { + // Upper-left triangle (don't use h10) + delta = std::max(std::max(h00, h01), h11) - + std::min(std::min(h00, h01), h11); + } + else + { + // Lower-right triangle (don't use h01) + delta = std::max(std::max(h00, h10), h11) - + std::min(std::min(h00, h10), h11); + } + } + + // Compute fractional slope (being careful to avoid intermediate overflows) + return fixed::FromInt(delta / TERRAIN_TILE_SIZE) / (int)HEIGHT_UNITS_PER_METRE; +} + float CTerrain::GetFilteredGroundLevel(float x, float z, float radius) const { // convert to [0,1] interval diff --git a/source/graphics/Terrain.h b/source/graphics/Terrain.h index 624cef3326..a2073d6033 100644 --- a/source/graphics/Terrain.h +++ b/source/graphics/Terrain.h @@ -87,9 +87,13 @@ public: fixed GetExactGroundLevelFixed(fixed x, fixed z) const; float GetFilteredGroundLevel(float x, float z, float radius) const; - // get the approximate slope (0 = horizontal, 0.5 = 30 degrees, 1.0 = 45 degrees, etc) + // get the approximate slope of a tile + // (0 = horizontal, 0.5 = 30 degrees, 1.0 = 45 degrees, etc) fixed GetSlopeFixed(ssize_t i, ssize_t j) const; + // get the precise slope of a point, accounting for triangulation direction + fixed GetExactSlopeFixed(fixed x, fixed z) const; + // Returns true if the triangulation diagonal for tile (i, j) // should be in the direction (1,-1); false if it should be (1,1) bool GetTriangulationDir(ssize_t i, ssize_t j) const; diff --git a/source/graphics/TerritoryBoundary.cpp b/source/graphics/TerritoryBoundary.cpp index 8c49992ae0..34d4ae4592 100644 --- a/source/graphics/TerritoryBoundary.cpp +++ b/source/graphics/TerritoryBoundary.cpp @@ -21,6 +21,7 @@ #include // for reverse #include "graphics/Terrain.h" +#include "simulation2/helpers/Pathfinding.h" #include "simulation2/components/ICmpTerritoryManager.h" std::vector CTerritoryBoundaryCalculator::ComputeBoundaries(const Grid* territory) @@ -123,9 +124,12 @@ std::vector CTerritoryBoundaryCalculator::ComputeBoundaries( u16 maxi = (u16)(grid.m_W-1); u16 maxj = (u16)(grid.m_H-1); + // Size of a territory tile in metres + float territoryTileSize = (Pathfinding::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE).ToFloat(); + while (true) { - points.push_back((CVector2D(ci, cj) + edgeOffsets[cdir]) * TERRAIN_TILE_SIZE); + points.push_back((CVector2D(ci, cj) + edgeOffsets[cdir]) * territoryTileSize); // Given that we're on an edge on a continuous boundary and aiming anticlockwise, // we can either carry on straight or turn left or turn right, so examine each diff --git a/source/graphics/TerritoryTexture.cpp b/source/graphics/TerritoryTexture.cpp index ad5a3623fb..ed00782e8e 100644 --- a/source/graphics/TerritoryTexture.cpp +++ b/source/graphics/TerritoryTexture.cpp @@ -25,6 +25,7 @@ #include "ps/Shapes.h" #include "renderer/Renderer.h" #include "simulation2/Simulation2.h" +#include "simulation2/helpers/Pathfinding.h" #include "simulation2/components/ICmpPlayer.h" #include "simulation2/components/ICmpPlayerManager.h" #include "simulation2/components/ICmpTerrain.h" @@ -92,7 +93,8 @@ void CTerritoryTexture::ConstructTexture(int unit) if (!cmpTerrain) return; - m_MapSize = cmpTerrain->GetVerticesPerSide() - 1; + // Convert size from terrain tiles to territory tiles + m_MapSize = cmpTerrain->GetTilesPerSide() * Pathfinding::NAVCELLS_PER_TILE / ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE; m_TextureSize = (GLsizei)round_up_to_pow2((size_t)m_MapSize); diff --git a/source/maths/Fixed.h b/source/maths/Fixed.h index ebd9418e8a..68a4cd0332 100644 --- a/source/maths/Fixed.h +++ b/source/maths/Fixed.h @@ -245,6 +245,19 @@ public: return CFixed(-value); } + CFixed operator>>(int n) const + { + ASSERT(n >= 0 && n < 32); + return CFixed(value >> n); + } + + CFixed operator<<(int n) const + { + ASSERT(n >= 0 && n < 32); + // TODO: check for overflow + return CFixed(value << n); + } + /// Divide by a CFixed. Must not have n.IsZero(). Might overflow. CFixed operator/(CFixed n) const { @@ -262,6 +275,14 @@ public: return CFixed(value * n); } + /// Multiply by an integer. Avoids overflow by clamping to min/max representable value. + CFixed MultiplyClamp(int n) const + { + i64 t = (i64)value * n; + t = std::max((i64)std::numeric_limits::min(), std::min((i64)std::numeric_limits::max(), t)); + return CFixed((i32)t); + } + /// Divide by an integer. Must not have n == 0. Cannot overflow unless n == -1. CFixed operator/(int n) const { diff --git a/source/maths/FixedVector2D.h b/source/maths/FixedVector2D.h index 7087fcc5f9..9e6e34b71b 100644 --- a/source/maths/FixedVector2D.h +++ b/source/maths/FixedVector2D.h @@ -79,6 +79,12 @@ public: return CFixedVector2D(X*n, Y*n); } + /// Scalar division by an integer. Must not have n == 0. + CFixedVector2D operator/(int n) const + { + return CFixedVector2D(X/n, Y/n); + } + /** * Multiply by a CFixed. Likely to overflow if both numbers are large, * so we use an ugly name instead of operator* to make it obvious. diff --git a/source/renderer/TerrainOverlay.cpp b/source/renderer/TerrainOverlay.cpp index 71c2532648..44f67fbd43 100644 --- a/source/renderer/TerrainOverlay.cpp +++ b/source/renderer/TerrainOverlay.cpp @@ -331,3 +331,41 @@ void TerrainTextureOverlay::RenderAfterWater(int cullGroup) g_Renderer.GetTerrainRenderer().RenderTerrainOverlayTexture(cullGroup, matrix); } + +SColor4ub TerrainTextureOverlay::GetColor(size_t idx, u8 alpha) const +{ + static u8 colors[][3] = { + { 255, 0, 0 }, + { 0, 255, 0 }, + { 0, 0, 255 }, + { 255, 255, 0 }, + { 255, 0, 255 }, + { 0, 255, 255 }, + { 255, 255, 255 }, + + { 127, 0, 0 }, + { 0, 127, 0 }, + { 0, 0, 127 }, + { 127, 127, 0 }, + { 127, 0, 127 }, + { 0, 127, 127 }, + { 127, 127, 127}, + + { 255, 127, 0 }, + { 127, 255, 0 }, + { 255, 0, 127 }, + { 127, 0, 255}, + { 0, 255, 127 }, + { 0, 127, 255}, + { 255, 127, 127}, + { 127, 255, 127}, + { 127, 127, 255}, + + { 127, 255, 255 }, + { 255, 127, 255 }, + { 255, 255, 127 }, + }; + + size_t c = idx % ARRAY_SIZE(colors); + return SColor4ub(colors[c][0], colors[c][1], colors[c][2], alpha); +} diff --git a/source/renderer/TerrainOverlay.h b/source/renderer/TerrainOverlay.h index 01cd91ed31..9125afb5ea 100644 --- a/source/renderer/TerrainOverlay.h +++ b/source/renderer/TerrainOverlay.h @@ -26,6 +26,7 @@ #include "lib/ogl.h" struct CColor; +struct SColor4ub; class CTerrain; class CSimContext; @@ -186,6 +187,12 @@ protected: */ virtual void BuildTextureRGBA(u8* data, size_t w, size_t h) = 0; + /** + * Returns an arbitrary color, for subclasses that want to distinguish + * different integers visually. + */ + SColor4ub GetColor(size_t idx, u8 alpha) const; + private: void RenderAfterWater(int cullGroup); diff --git a/source/renderer/TerrainRenderer.cpp b/source/renderer/TerrainRenderer.cpp index 9e96ea6cd5..dc6887b9c2 100644 --- a/source/renderer/TerrainRenderer.cpp +++ b/source/renderer/TerrainRenderer.cpp @@ -394,6 +394,7 @@ void TerrainRenderer::RenderTerrainOverlayTexture(int cullGroup, CMatrix3D& text glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glDepthMask(0); + glDisable(GL_DEPTH_TEST); glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE); @@ -433,6 +434,7 @@ void TerrainRenderer::RenderTerrainOverlayTexture(int cullGroup, CMatrix3D& text glMatrixMode(GL_MODELVIEW); glDepthMask(1); + glEnable(GL_DEPTH_TEST); glDisable(GL_BLEND); glDisableClientState(GL_COLOR_ARRAY); glDisableClientState(GL_VERTEX_ARRAY); diff --git a/source/simulation2/MessageTypes.h b/source/simulation2/MessageTypes.h index 33a03d2e6a..fa9a52115f 100644 --- a/source/simulation2/MessageTypes.h +++ b/source/simulation2/MessageTypes.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2014 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -458,13 +458,13 @@ class CMessagePathResult : public CMessage public: DEFAULT_MESSAGE_IMPL(PathResult) - CMessagePathResult(u32 ticket, const ICmpPathfinder::Path& path) : + CMessagePathResult(u32 ticket, const WaypointPath& path) : ticket(ticket), path(path) { } u32 ticket; - ICmpPathfinder::Path path; + WaypointPath path; }; /** diff --git a/source/simulation2/Simulation2.cpp b/source/simulation2/Simulation2.cpp index ac061d1dda..c48c83597e 100644 --- a/source/simulation2/Simulation2.cpp +++ b/source/simulation2/Simulation2.cpp @@ -485,7 +485,10 @@ void CSimulation2Impl::UpdateComponents(CSimContext& simContext, fixed turnLengt CmpPtr cmpPathfinder(simContext, SYSTEM_ENTITY); if (cmpPathfinder) + { + cmpPathfinder->UpdateGrid(); cmpPathfinder->FinishAsyncRequests(); + } // Push AI commands onto the queue before we use them CmpPtr cmpAIManager(simContext, SYSTEM_ENTITY); diff --git a/source/simulation2/components/CCmpAIManager.cpp b/source/simulation2/components/CCmpAIManager.cpp index dd5540d785..4752add93e 100644 --- a/source/simulation2/components/CCmpAIManager.cpp +++ b/source/simulation2/components/CCmpAIManager.cpp @@ -36,7 +36,7 @@ #include "simulation2/components/ICmpTemplateManager.h" #include "simulation2/components/ICmpTechnologyTemplateManager.h" #include "simulation2/components/ICmpTerritoryManager.h" -#include "simulation2/helpers/Grid.h" +#include "simulation2/helpers/LongPathfinder.h" #include "simulation2/serialization/DebugSerializer.h" #include "simulation2/serialization/StdDeserializer.h" #include "simulation2/serialization/StdSerializer.h" @@ -223,6 +223,9 @@ public: m_ScriptInterface->RegisterFunction("IncludeModule"); m_ScriptInterface->RegisterFunction("DumpHeap"); m_ScriptInterface->RegisterFunction("ForceGC"); + + m_ScriptInterface->RegisterFunction("ComputePath"); + m_ScriptInterface->RegisterFunction("GetConnectivityGrid"); m_ScriptInterface->RegisterFunction, u32, u32, u32, CAIWorker::DumpImage>("DumpImage"); } @@ -288,6 +291,48 @@ public: LOGERROR("Invalid playerid in PostCommand!"); } + + static JS::Value ComputePath(ScriptInterface::CxPrivate* pCxPrivate, + JS::HandleValue position, JS::HandleValue goal, pass_class_t passClass) + { + ENSURE(pCxPrivate->pCBData); + CAIWorker* self = static_cast (pCxPrivate->pCBData); + JSContext* cx(self->m_ScriptInterface->GetContext()); + + CFixedVector2D pos, goalPos; + std::vector waypoints; + JS::RootedValue retVal(cx); + + self->m_ScriptInterface->FromJSVal(cx, position, pos); + self->m_ScriptInterface->FromJSVal(cx, goal, goalPos); + + self->ComputePath(pos, goalPos, passClass, waypoints); + self->m_ScriptInterface->ToJSVal >(cx, &retVal, waypoints); + + return retVal; + } + + void ComputePath(const CFixedVector2D& pos, const CFixedVector2D& goal, pass_class_t passClass, std::vector& waypoints) + { + WaypointPath ret; + PathGoal pathGoal = { PathGoal::POINT, goal.X, goal.Y }; + m_LongPathfinder.ComputePath(pos.X, pos.Y, pathGoal, passClass, ret); + + for (Waypoint& wp : ret.m_Waypoints) + waypoints.emplace_back(wp.x, wp.z); + } + + static JS::Value GetConnectivityGrid(ScriptInterface::CxPrivate* pCxPrivate, pass_class_t passClass) + { + ENSURE(pCxPrivate->pCBData); + CAIWorker* self = static_cast (pCxPrivate->pCBData); + JSContext* cx(self->m_ScriptInterface->GetContext()); + + JS::RootedValue retVal(cx); + self->m_ScriptInterface->ToJSVal >(cx, &retVal, self->m_LongPathfinder.GetConnectivityGrid(passClass)); + return retVal; + } + // The next two ought to be implmeneted someday but for now as it returns "null" it can't static void DumpHeap(ScriptInterface::CxPrivate* pCxPrivate) { @@ -465,16 +510,23 @@ public: return true; } - void StartComputation(const shared_ptr& gameState, const Grid& passabilityMap, const Grid& territoryMap, bool territoryMapDirty) + void StartComputation(const shared_ptr& gameState, + const Grid& passabilityMap, bool passabilityMapDirty, const Grid* dirtinessGrid, bool passabilityMapEntirelyDirty, + const Grid& territoryMap, bool territoryMapDirty, + std::map passClassMasks) { ENSURE(m_CommandsComputed); m_GameState = gameState; JSContext* cx = m_ScriptInterface->GetContext(); - if (passabilityMap.m_DirtyID != m_PassabilityMap.m_DirtyID) + if (passabilityMapDirty) { m_PassabilityMap = passabilityMap; + if (passabilityMapEntirelyDirty) + m_LongPathfinder.Reload(passClassMasks, &m_PassabilityMap); + else + m_LongPathfinder.Update(&m_PassabilityMap, dirtinessGrid); ScriptInterface::ToJSVal(cx, &m_PassabilityMapVal, m_PassabilityMap); } @@ -794,6 +846,8 @@ private: Grid m_TerritoryMap; JS::PersistentRootedValue m_TerritoryMapVal; + LongPathfinder m_LongPathfinder; + bool m_CommandsComputed; shared_ptr > m_SerializablePrototypes; @@ -981,6 +1035,11 @@ public: if (cmpPathfinder) passabilityMap = &cmpPathfinder->GetPassabilityGrid(); + Grid dummyDirtinessGrid; + const Grid* dirtinessGrid = &dummyDirtinessGrid; + bool passabilityMapEntirelyDirty = false; + bool passabilityMapDirty = cmpPathfinder->GetDirtinessData(dummyDirtinessGrid, passabilityMapEntirelyDirty); + // Get the territory data // Since getting the territory grid can trigger a recalculation, we check NeedUpdate first bool territoryMapDirty = false; @@ -994,8 +1053,14 @@ public: } LoadPathfinderClasses(state); + std::map passClassMasks; + if (cmpPathfinder) + passClassMasks = cmpPathfinder->GetPassabilityClasses(); - m_Worker.StartComputation(scriptInterface.WriteStructuredClone(state), *passabilityMap, *territoryMap, territoryMapDirty); + m_Worker.StartComputation(scriptInterface.WriteStructuredClone(state), + *passabilityMap, passabilityMapDirty, dirtinessGrid, passabilityMapEntirelyDirty, + *territoryMap, territoryMapDirty, + passClassMasks); m_JustDeserialized = false; } @@ -1082,10 +1147,10 @@ private: JSAutoRequest rq(cx); JS::RootedValue classesVal(cx); - scriptInterface.Eval("({ pathfinderObstruction: 1, foundationObstruction: 2 })", &classesVal); + scriptInterface.Eval("({})", &classesVal); - std::map classes = cmpPathfinder->GetPassabilityClasses(); - for (std::map::iterator it = classes.begin(); it != classes.end(); ++it) + std::map classes = cmpPathfinder->GetPassabilityClasses(); + for (std::map::iterator it = classes.begin(); it != classes.end(); ++it) scriptInterface.SetProperty(classesVal, it->first.c_str(), it->second, true); scriptInterface.SetProperty(state, "passabilityClasses", classesVal, true); diff --git a/source/simulation2/components/CCmpFootprint.cpp b/source/simulation2/components/CCmpFootprint.cpp index b139e3603c..3c99f60368 100644 --- a/source/simulation2/components/CCmpFootprint.cpp +++ b/source/simulation2/components/CCmpFootprint.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2013 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -154,7 +154,7 @@ public: if (!cmpUnitMotion) return error; - ICmpPathfinder::pass_class_t spawnedPass = cmpUnitMotion->GetPassabilityClass(); + pass_class_t spawnedPass = cmpUnitMotion->GetPassabilityClass(); CmpPtr cmpPathfinder(GetSystemEntity()); if (!cmpPathfinder) return error; @@ -285,7 +285,7 @@ public: if (!cmpUnitMotion) return error; - ICmpPathfinder::pass_class_t spawnedPass = cmpUnitMotion->GetPassabilityClass(); + pass_class_t spawnedPass = cmpUnitMotion->GetPassabilityClass(); CmpPtr cmpPathfinder(GetSystemEntity()); if (!cmpPathfinder) return error; @@ -294,7 +294,7 @@ public: CmpPtr cmpEntityMotion(GetEntityHandle()); if (!cmpEntityMotion) return error; - ICmpPathfinder::pass_class_t entityPass = cmpEntityMotion->GetPassabilityClass(); + pass_class_t entityPass = cmpEntityMotion->GetPassabilityClass(); CFixedVector2D initialPos = cmpPosition->GetPosition2D(); entity_angle_t initialAngle = cmpPosition->GetRotation().Y; diff --git a/source/simulation2/components/CCmpObstruction.cpp b/source/simulation2/components/CCmpObstruction.cpp index 3e0b42dece..56a5fe9f0d 100644 --- a/source/simulation2/components/CCmpObstruction.cpp +++ b/source/simulation2/components/CCmpObstruction.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2013 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -58,6 +58,7 @@ public: entity_pos_t m_Size0; // radius or width entity_pos_t m_Size1; // radius or depth flags_t m_TemplateFlags; + entity_pos_t m_Clearance; typedef struct { entity_pos_t dx, dz; @@ -112,10 +113,14 @@ public: "" "" "" - "" + "" + "1.5" + "" "" "" - "" + "" + "1.5" + "" "" "" "" @@ -138,10 +143,14 @@ public: "" "" "" - "" + "" + "1.5" + "" "" "" - "" + "" + "1.5" + "" "" "" "" @@ -177,6 +186,9 @@ public: virtual void Init(const CParamNode& paramNode) { + // The minimum obstruction size is the navcell size * sqrt(2) + // This is enforced in the schema as a minimum of 1.5 + fixed minObstruction = (Pathfinding::NAVCELL_SIZE.Square() * 2).Sqrt(); m_TemplateFlags = 0; if (paramNode.GetChild("BlockMovement").ToBool()) m_TemplateFlags |= ICmpObstructionManager::FLAG_BLOCK_MOVEMENT; @@ -203,6 +215,8 @@ public: m_Type = STATIC; m_Size0 = paramNode.GetChild("Static").GetChild("@width").ToFixed(); m_Size1 = paramNode.GetChild("Static").GetChild("@depth").ToFixed(); + ENSURE(m_Size0 > minObstruction); + ENSURE(m_Size1 > minObstruction); } else { @@ -215,6 +229,8 @@ public: Shape b; b.size0 = it->second.GetChild("@width").ToFixed(); b.size1 = it->second.GetChild("@depth").ToFixed(); + ENSURE(b.size0 > minObstruction); + ENSURE(b.size1 > minObstruction); b.dx = it->second.GetChild("@x").ToFixed(); b.dz = it->second.GetChild("@z").ToFixed(); b.da = entity_angle_t::FromInt(0); @@ -319,7 +335,7 @@ public: data.x, data.z, data.a, m_Size0, m_Size1, m_Flags, m_ControlGroup, m_ControlGroup2); else if (m_Type == UNIT) m_Tag = cmpObstructionManager->AddUnitShape(GetEntityId(), - data.x, data.z, m_Size0, (flags_t)(m_Flags | (m_Moving ? ICmpObstructionManager::FLAG_MOVING : 0)), m_ControlGroup); + data.x, data.z, m_Size0, m_Clearance, (flags_t)(m_Flags | (m_Moving ? ICmpObstructionManager::FLAG_MOVING : 0)), m_ControlGroup); else AddClusterShapes(data.x, data.x, data.a); } @@ -376,7 +392,7 @@ public: pos.X, pos.Y, cmpPosition->GetRotation().Y, m_Size0, m_Size1, m_Flags, m_ControlGroup, m_ControlGroup2); else if (m_Type == UNIT) m_Tag = cmpObstructionManager->AddUnitShape(GetEntityId(), - pos.X, pos.Y, m_Size0, (flags_t)(m_Flags | (m_Moving ? ICmpObstructionManager::FLAG_MOVING : 0)), m_ControlGroup); + pos.X, pos.Y, m_Size0, m_Clearance, (flags_t)(m_Flags | (m_Moving ? ICmpObstructionManager::FLAG_MOVING : 0)), m_ControlGroup); else AddClusterShapes(pos.X, pos.Y, cmpPosition->GetRotation().Y); } @@ -493,6 +509,12 @@ public: return CFixedVector2D(m_Size0 / 2, m_Size1 / 2).Length(); } + virtual void SetUnitClearance(const entity_pos_t& clearance) + { + if (m_Type == UNIT) + m_Clearance = clearance; + } + virtual bool IsControlPersistent() { return m_ControlPersist; @@ -526,7 +548,7 @@ public: } // Get passability class - ICmpPathfinder::pass_class_t passClass = cmpPathfinder->GetPassabilityClass(className); + pass_class_t passClass = cmpPathfinder->GetPassabilityClass(className); // Ignore collisions within the same control group, or with other non-foundation-blocking shapes. // Note that, since the control group for each entity defaults to the entity's ID, this is typically @@ -574,28 +596,15 @@ public: virtual std::vector GetEntityCollisions(bool checkStructures, bool checkUnits) { + // TODO: That function actually only checks for overlapping units when a new buidling is started. + // The name of the function should be changed and useless code removed. + std::vector ret; - CmpPtr cmpPosition(GetEntityHandle()); - if (!cmpPosition) - return ret; // error - - if (!cmpPosition->IsInWorld()) - return ret; // no obstruction - - CFixedVector2D pos = cmpPosition->GetPosition2D(); - CmpPtr cmpObstructionManager(GetSystemEntity()); if (!cmpObstructionManager) return ret; // error - // required precondition to use SkipControlGroupsRequireFlagObstructionFilter - if (m_ControlGroup == INVALID_ENTITY) - { - LOGERROR("[CmpObstruction] Cannot test for unit or structure obstructions; primary control group must be valid"); - return ret; - } - flags_t flags = 0; bool invertMatch = false; @@ -619,18 +628,16 @@ public: } // Ignore collisions within the same control group, or with other shapes that don't match the filter. - // Note that, since the control group for each entity defaults to the entity's ID, this is typically + // Note that, since the control group for each entity defaults to the entity's ID, this is typically // equivalent to only ignoring the entity's own shape and other shapes that don't match the filter. - SkipControlGroupsRequireFlagObstructionFilter filter(invertMatch, - m_ControlGroup, m_ControlGroup2, flags); + SkipControlGroupsRequireFlagObstructionFilter filter(invertMatch, m_ControlGroup, m_ControlGroup2, flags); + + ICmpObstructionManager::ObstructionSquare square; + if (!GetObstructionSquare(square)) + return ret; // error + + cmpObstructionManager->GetUnitsOnObstruction(square, ret, filter); - if (m_Type == STATIC) - cmpObstructionManager->TestStaticShape(filter, pos.X, pos.Y, cmpPosition->GetRotation().Y, m_Size0, m_Size1, &ret); - else if (m_Type == UNIT) - cmpObstructionManager->TestUnitShape(filter, pos.X, pos.Y, m_Size0, &ret); - else - cmpObstructionManager->TestStaticShape(filter, pos.X, pos.Y, cmpPosition->GetRotation().Y, m_Size0, m_Size1, &ret); - return ret; } diff --git a/source/simulation2/components/CCmpObstructionManager.cpp b/source/simulation2/components/CCmpObstructionManager.cpp index 90a3d63714..ec1dbae9ca 100644 --- a/source/simulation2/components/CCmpObstructionManager.cpp +++ b/source/simulation2/components/CCmpObstructionManager.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2012 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -20,8 +20,11 @@ #include "simulation2/system/Component.h" #include "ICmpObstructionManager.h" +#include "ICmpTerrain.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" @@ -51,6 +54,7 @@ struct UnitShape entity_id_t entity; entity_pos_t x, z; entity_pos_t r; // radius of circle, or half width of square + 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) }; @@ -81,6 +85,7 @@ struct SerializeUnitShape serialize.NumberFixed_Unbounded("x", value.x); serialize.NumberFixed_Unbounded("z", value.z); serialize.NumberFixed_Unbounded("r", value.r); + serialize.NumberFixed_Unbounded("clearance", value.clearance); serialize.NumberU8_Unbounded("flags", value.flags); serialize.NumberU32_Unbounded("group", value.group); } @@ -152,7 +157,9 @@ public: m_UnitShapeNext = 1; m_StaticShapeNext = 1; - m_DirtyID = 1; // init to 1 so default-initialised grids are considered dirty + m_Dirty = true; + m_NeedsGlobalUpdate = true; + m_DirtinessGrid = NULL; m_PassabilityCircular = false; @@ -165,6 +172,7 @@ public: virtual void Deinit() { + SAFE_DELETE(m_DirtinessGrid); } template @@ -225,6 +233,15 @@ public: // Subdivision system bounds: ENSURE(x0.IsZero() && z0.IsZero()); // don't bother implementing non-zero offsets yet ResetSubdivisions(x1, z1); + + SAFE_DELETE(m_DirtinessGrid); + + CmpPtr cmpTerrain(GetSystemEntity()); + if (!cmpTerrain) + return; + + u16 tiles = cmpTerrain->GetTilesPerSide(); + m_DirtinessGrid = new Grid(tiles*Pathfinding::NAVCELLS_PER_TILE, tiles*Pathfinding::NAVCELLS_PER_TILE); } void ResetSubdivisions(entity_pos_t x1, entity_pos_t z1) @@ -249,12 +266,12 @@ public: } } - virtual tag_t AddUnitShape(entity_id_t ent, entity_pos_t x, entity_pos_t z, entity_pos_t r, flags_t flags, entity_id_t group) + virtual tag_t AddUnitShape(entity_id_t ent, entity_pos_t x, entity_pos_t z, entity_pos_t r, entity_pos_t clearance, flags_t flags, entity_id_t group) { - UnitShape shape = { ent, x, z, r, flags, group }; + UnitShape shape = { ent, x, z, r, clearance, flags, group }; u32 id = m_UnitShapeNext++; m_UnitShapes[id] = shape; - MakeDirtyUnit(flags); + MakeDirtyUnit(flags, id); m_UnitSubdivision.Add(id, CFixedVector2D(x - r, z - r), CFixedVector2D(x + r, z + r)); @@ -271,7 +288,7 @@ public: StaticShape shape = { ent, x, z, u, v, w/2, h/2, flags, group, group2 }; u32 id = m_StaticShapeNext++; m_StaticShapes[id] = shape; - MakeDirtyStatic(flags); + MakeDirtyStatic(flags, id); CFixedVector2D center(x, z); CFixedVector2D bbHalfSize = Geometry::GetHalfBoundingBox(u, v, CFixedVector2D(w/2, h/2)); @@ -316,7 +333,7 @@ public: shape.x = x; shape.z = z; - MakeDirtyUnit(shape.flags); + MakeDirtyUnit(shape.flags, TAG_TO_INDEX(tag)); } else { @@ -340,7 +357,7 @@ public: shape.u = u; shape.v = v; - MakeDirtyStatic(shape.flags); + MakeDirtyStatic(shape.flags, TAG_TO_INDEX(tag)); } } @@ -394,7 +411,7 @@ public: CFixedVector2D(shape.x - shape.r, shape.z - shape.r), CFixedVector2D(shape.x + shape.r, shape.z + shape.r)); - MakeDirtyUnit(shape.flags); + MakeDirtyUnit(shape.flags, TAG_TO_INDEX(tag)); m_UnitShapes.erase(TAG_TO_INDEX(tag)); } else @@ -405,7 +422,7 @@ public: 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); + MakeDirtyStatic(shape.flags, TAG_TO_INDEX(tag)); m_StaticShapes.erase(TAG_TO_INDEX(tag)); } } @@ -434,9 +451,9 @@ public: 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); virtual bool TestUnitShape(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, std::vector* out); - virtual bool Rasterise(Grid& grid); + virtual void Rasterize(Grid& grid, const entity_pos_t& expand, ICmpObstructionManager::flags_t requireMask, u16 setMask); 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); - virtual bool FindMostImportantObstruction(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, ObstructionSquare& square); + virtual void GetUnitsOnObstruction(const ObstructionSquare& square, std::vector& out, const IObstructionTestFilter& filter); virtual void SetPassabilityCircular(bool enabled) { @@ -447,6 +464,11 @@ public: GetSimContext().GetComponentManager().BroadcastMessage(msg); } + virtual bool GetPassabilityCircular() const + { + return m_PassabilityCircular; + } + virtual void SetDebugOverlay(bool enabled) { m_DebugOverlayEnabled = enabled; @@ -457,20 +479,38 @@ public: void RenderSubmit(SceneCollector& collector); -private: - // To support lazy updates of grid rasterisations of obstruction data, - // we maintain a DirtyID here and increment it whenever obstructions change; - // if a grid has a lower DirtyID then it needs to be updated. + virtual bool GetDirtinessData(Grid& dirtinessGrid, bool& globalUpdateNeeded) + { + if (!m_Dirty || !m_DirtinessGrid) + { + globalUpdateNeeded = false; + return false; + } - size_t m_DirtyID; + dirtinessGrid = *m_DirtinessGrid; + globalUpdateNeeded = m_NeedsGlobalUpdate; + + m_Dirty = false; + m_NeedsGlobalUpdate = false; + m_DirtinessGrid->reset(); + + return true; + } + +private: + // Dynamic updates for the long-range pathfinder + bool m_Dirty; + bool m_NeedsGlobalUpdate; + Grid* m_DirtinessGrid; /** - * Mark all previous Rasterise()d grids as dirty, and the debug display. + * Mark all previous Rasterize()d grids as dirty, and the debug display. * Call this when the world bounds have changed. */ void MakeDirtyAll() { - ++m_DirtyID; + m_Dirty = true; + m_NeedsGlobalUpdate = true; m_DebugOverlayDirty = true; } @@ -483,39 +523,56 @@ private: m_DebugOverlayDirty = true; } + inline void MarkDirtinessGrid(const entity_pos_t& x, const entity_pos_t& z, const entity_pos_t& r) + { + if (!m_DirtinessGrid) + return; + + u16 j0, j1, i0, i1; + Pathfinding::NearestNavcell(x - r, z - r, i0, j0, m_DirtinessGrid->m_W, m_DirtinessGrid->m_H); + Pathfinding::NearestNavcell(x + r, z + r, i1, j1, m_DirtinessGrid->m_W, m_DirtinessGrid->m_H); + + for (int j = j0; j < j1; ++j) + for (int i = i0; i < i1; ++i) + m_DirtinessGrid->set(i, j, 1); + } + /** - * Mark all previous Rasterise()d grids as dirty, if they depend on this shape. + * 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) + void MakeDirtyStatic(flags_t flags, u32 index) { - if (flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION)) - ++m_DirtyID; + if (flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION)) + { + m_Dirty = true; + + auto it = m_StaticShapes.find(index); + if (it != m_StaticShapes.end()) + MarkDirtinessGrid(it->second.x, it->second.z, std::max(it->second.hw, it->second.hh)); + } m_DebugOverlayDirty = true; } /** - * Mark all previous Rasterise()d grids as dirty, if they depend on this shape. + * 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) + void MakeDirtyUnit(flags_t flags, u32 index) { - if (flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION)) - ++m_DirtyID; + if (flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION)) + { + m_Dirty = true; + + auto it = m_UnitShapes.find(index); + if (it != m_UnitShapes.end()) + MarkDirtinessGrid(it->second.x, it->second.z, it->second.r); + } m_DebugOverlayDirty = true; } - /** - * Test whether a Rasterise()d grid is dirty and needs updating - */ - template - bool IsDirty(const Grid& grid) - { - return grid.m_DirtyID < m_DirtyID; - } - /** * Return whether the given point is within the world bounds by at least r */ @@ -706,176 +763,61 @@ bool CCmpObstructionManager::TestUnitShape(const IObstructionTestFilter& filter, return false; // didn't collide, if we got this far } -/** - * Compute the tile indexes on the grid nearest to a given point - */ -static void NearestTile(entity_pos_t x, entity_pos_t z, u16& i, u16& j, u16 w, u16 h) +void CCmpObstructionManager::Rasterize(Grid& grid, const entity_pos_t& expand, ICmpObstructionManager::flags_t requireMask, u16 setMask) { - i = (u16)clamp((x / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, w-1); - j = (u16)clamp((z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, h-1); -} + PROFILE3("Rasterize"); -/** - * Returns the position of the center of the given tile - */ -static void TileCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z) -{ - x = entity_pos_t::FromInt(i*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE/2); - z = entity_pos_t::FromInt(j*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE/2); -} + // Since m_DirtyID is only updated for pathfinding/foundation blocking shapes, + // NeedUpdate+Rasterize will only be accurate for that subset of shapes. + // (If we ever want to support rasterizing more shapes, we need to improve + // the dirty-detection system too.) + ENSURE(!(requireMask & ~(FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION))); -bool CCmpObstructionManager::Rasterise(Grid& grid) -{ - if (!IsDirty(grid)) - return false; + // 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.) - PROFILE("Rasterise"); + // TODO: it might be nice to rasterize with rounded corners for large 'expand' values. - grid.m_DirtyID = m_DirtyID; + // (This could be implemented much more efficiently.) - // TODO: this is all hopelessly inefficient - // What we should perhaps do is have some kind of quadtree storing Shapes so it's - // quick to invalidate and update small numbers of tiles - - grid.reset(); - - // For tile-based pathfinding: - // Since we only count tiles whose centers are inside the square, - // we maybe want to expand the square a bit so we're less likely to think there's - // free space between buildings when there isn't. But this is just a random guess - // and needs to be tweaked until everything works nicely. - //entity_pos_t expandPathfinding = entity_pos_t::FromInt(TERRAIN_TILE_SIZE / 2); - // Actually that's bad because units get stuck when the A* pathfinder thinks they're - // blocked on all sides, so it's better to underestimate - entity_pos_t expandPathfinding = entity_pos_t::FromInt(0); - - // For AI building foundation planning, we want to definitely block all - // potentially-obstructed tiles (so we don't blindly build on top of an obstruction), - // so we need to expand by at least 1/sqrt(2) of a tile - entity_pos_t expandFoundation = (entity_pos_t::FromInt(TERRAIN_TILE_SIZE) * 3) / 4; - - for (std::map::iterator it = m_StaticShapes.begin(); it != m_StaticShapes.end(); ++it) + for (auto& pair : m_StaticShapes) { - CFixedVector2D center(it->second.x, it->second.z); + const StaticShape& shape = pair.second; + if (!(shape.flags & requireMask)) + continue; - if (it->second.flags & FLAG_BLOCK_PATHFINDING) + ObstructionSquare square = { shape.x, shape.z, shape.u, shape.v, shape.hw, shape.hh }; + SimRasterize::Spans spans; + SimRasterize::RasterizeRectWithClearance(spans, square, expand, Pathfinding::NAVCELL_SIZE); + for (SimRasterize::Span& span : spans) { - CFixedVector2D halfSize(it->second.hw + expandPathfinding, it->second.hh + expandPathfinding); - CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(it->second.u, it->second.v, halfSize); - - u16 i0, j0, i1, j1; - NearestTile(center.X - halfBound.X, center.Y - halfBound.Y, i0, j0, grid.m_W, grid.m_H); - NearestTile(center.X + halfBound.X, center.Y + halfBound.Y, i1, j1, grid.m_W, grid.m_H); - for (u16 j = j0; j <= j1; ++j) + i16 j = span.j; + if (j >= 0 && j <= grid.m_H) { - for (u16 i = i0; i <= i1; ++i) - { - entity_pos_t x, z; - TileCenter(i, j, x, z); - if (Geometry::PointIsInSquare(CFixedVector2D(x, z) - center, it->second.u, it->second.v, halfSize)) - grid.set(i, j, grid.get(i, j) | TILE_OBSTRUCTED_PATHFINDING); - } - } - } - - if (it->second.flags & FLAG_BLOCK_FOUNDATION) - { - CFixedVector2D halfSize(it->second.hw + expandFoundation, it->second.hh + expandFoundation); - CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(it->second.u, it->second.v, halfSize); - - u16 i0, j0, i1, j1; - NearestTile(center.X - halfBound.X, center.Y - halfBound.Y, i0, j0, grid.m_W, grid.m_H); - NearestTile(center.X + halfBound.X, center.Y + halfBound.Y, i1, j1, grid.m_W, grid.m_H); - 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, z) - center, it->second.u, it->second.v, halfSize)) - grid.set(i, j, grid.get(i, j) | TILE_OBSTRUCTED_FOUNDATION); - } + 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) | setMask); } } } - for (std::map::iterator it = m_UnitShapes.begin(); it != m_UnitShapes.end(); ++it) + for (auto& pair : m_UnitShapes) { - CFixedVector2D center(it->second.x, it->second.z); + CFixedVector2D center(pair.second.x, pair.second.z); - if (it->second.flags & FLAG_BLOCK_PATHFINDING) - { - entity_pos_t r = it->second.r + expandPathfinding; + if (!(pair.second.flags & requireMask)) + continue; - u16 i0, j0, i1, j1; - NearestTile(center.X - r, center.Y - r, i0, j0, grid.m_W, grid.m_H); - NearestTile(center.X + r, center.Y + r, i1, j1, grid.m_W, grid.m_H); - for (u16 j = j0; j <= j1; ++j) - for (u16 i = i0; i <= i1; ++i) - grid.set(i, j, grid.get(i, j) | TILE_OBSTRUCTED_PATHFINDING); - } + entity_pos_t r = pair.second.r + expand; - if (it->second.flags & FLAG_BLOCK_FOUNDATION) - { - entity_pos_t r = it->second.r + expandFoundation; - - u16 i0, j0, i1, j1; - NearestTile(center.X - r, center.Y - r, i0, j0, grid.m_W, grid.m_H); - NearestTile(center.X + r, center.Y + r, i1, j1, grid.m_W, grid.m_H); - for (u16 j = j0; j <= j1; ++j) - for (u16 i = i0; i <= i1; ++i) - grid.set(i, j, grid.get(i, j) | TILE_OBSTRUCTED_FOUNDATION); - } - } - - // Any tiles outside or very near the edge of the map are impassable - - // WARNING: CCmpRangeManager::LosIsOffWorld needs to be kept in sync with this - const u16 edgeSize = 3; // number of tiles around the edge that will be off-world - - u8 edgeFlags = TILE_OBSTRUCTED_PATHFINDING | TILE_OBSTRUCTED_FOUNDATION | TILE_OUTOFBOUNDS; - - if (m_PassabilityCircular) - { - for (u16 j = 0; j < grid.m_H; ++j) - { - for (u16 i = 0; i < grid.m_W; ++i) - { - // Based on CCmpRangeManager::LosIsOffWorld - // but tweaked since it's tile-based instead. - // (We double all the values so we can handle half-tile coordinates.) - // This needs to be slightly tighter than the LOS circle, - // else units might get themselves lost in the SoD around the edge. - - ssize_t dist2 = (i*2 + 1 - grid.m_W)*(i*2 + 1 - grid.m_W) - + (j*2 + 1 - grid.m_H)*(j*2 + 1 - grid.m_H); - - if (dist2 >= (grid.m_W - 2*edgeSize) * (grid.m_H - 2*edgeSize)) - grid.set(i, j, edgeFlags); - } - } - } - else - { u16 i0, j0, i1, j1; - NearestTile(m_WorldX0, m_WorldZ0, i0, j0, grid.m_W, grid.m_H); - NearestTile(m_WorldX1, m_WorldZ1, i1, j1, grid.m_W, grid.m_H); - - for (u16 j = 0; j < grid.m_H; ++j) - for (u16 i = 0; i < i0+edgeSize; ++i) - grid.set(i, j, edgeFlags); - for (u16 j = 0; j < grid.m_H; ++j) - for (u16 i = (u16)(i1-edgeSize+1); i < grid.m_W; ++i) - grid.set(i, j, edgeFlags); - for (u16 j = 0; j < j0+edgeSize; ++j) - for (u16 i = (u16)(i0+edgeSize); i < i1-edgeSize+1; ++i) - grid.set(i, j, edgeFlags); - for (u16 j = (u16)(j1-edgeSize+1); j < grid.m_H; ++j) - for (u16 i = (u16)(i0+edgeSize); i < i1-edgeSize+1; ++i) - grid.set(i, j, edgeFlags); + 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) | setMask); } - - return true; } 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) @@ -886,9 +828,9 @@ void CCmpObstructionManager::GetObstructionsInRange(const IObstructionTestFilter std::vector unitShapes; m_UnitSubdivision.GetInRange(unitShapes, CFixedVector2D(x0, z0), CFixedVector2D(x1, z1)); - for (size_t i = 0; i < unitShapes.size(); ++i) + for (entity_id_t& unitShape : unitShapes) { - std::map::iterator it = m_UnitShapes.find(unitShapes[i]); + auto 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)) @@ -902,15 +844,14 @@ void CCmpObstructionManager::GetObstructionsInRange(const IObstructionTestFilter CFixedVector2D u(entity_pos_t::FromInt(1), entity_pos_t::Zero()); CFixedVector2D v(entity_pos_t::Zero(), entity_pos_t::FromInt(1)); - ObstructionSquare s = { it->second.x, it->second.z, u, v, r, r }; - squares.push_back(s); + squares.emplace_back(ObstructionSquare{ it->second.x, it->second.z, u, v, r, r }); } std::vector staticShapes; m_StaticSubdivision.GetInRange(staticShapes, CFixedVector2D(x0, z0), CFixedVector2D(x1, z1)); - for (size_t i = 0; i < staticShapes.size(); ++i) + for (entity_id_t& staticShape : staticShapes) { - std::map::iterator it = m_StaticShapes.find(staticShapes[i]); + auto 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)) @@ -924,46 +865,65 @@ void CCmpObstructionManager::GetObstructionsInRange(const IObstructionTestFilter // TODO: maybe we should use Geometry::GetHalfBoundingBox to be more precise? - ObstructionSquare s = { it->second.x, it->second.z, it->second.u, it->second.v, it->second.hw, it->second.hh }; - squares.push_back(s); + squares.emplace_back(ObstructionSquare{ it->second.x, it->second.z, it->second.u, it->second.v, it->second.hw, it->second.hh }); } } -bool CCmpObstructionManager::FindMostImportantObstruction(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, ObstructionSquare& square) +void CCmpObstructionManager::GetUnitsOnObstruction(const ObstructionSquare& square, std::vector& out, const IObstructionTestFilter& filter) { - std::vector squares; + PROFILE3("GetUnitsOnObstruction"); - CFixedVector2D center(x, z); + // In order to avoid getting units on impassable cells, we want to find all + // units s.t. the RasterizeRectWithClearance of the building's shape with the + // unit's clearance covers the navcell the unit is on. - // First look for obstructions that are covering the exact target point - GetObstructionsInRange(filter, x, z, x, z, squares); - // Building squares are more important but returned last, so check backwards - for (std::vector::reverse_iterator it = squares.rbegin(); it != squares.rend(); ++it) + CmpPtr cmpPathfinder(GetSystemEntity()); + if (!cmpPathfinder) + return; + + entity_pos_t maxClearance = cmpPathfinder->GetMaximumClearance(); + + std::vector unitShapes; + CFixedVector2D center(square.x, square.z); + CFixedVector2D expandedBox = + Geometry::GetHalfBoundingBox(square.u, square.v, CFixedVector2D(square.hw, square.hh)) + + CFixedVector2D(maxClearance, maxClearance); + m_UnitSubdivision.GetInRange(unitShapes, center - expandedBox, center + expandedBox); + + std::map rasterizedRects; + + for (entity_id_t& unitShape : unitShapes) { - CFixedVector2D halfSize(it->hw, it->hh); - if (Geometry::PointIsInSquare(CFixedVector2D(it->x, it->z) - center, it->u, it->v, halfSize)) + auto it = m_UnitShapes.find(unitShape); + ENSURE(it != m_UnitShapes.end()); + + 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()) { - square = *it; - return true; + SimRasterize::Spans& newSpans = rasterizedRects[shape.clearance]; + SimRasterize::RasterizeRectWithClearance(newSpans, square, shape.clearance, 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 (size_t k = 0; k < spans.size(); ++k) + { + if (j == spans[k].j && spans[k].i0 <= i && i < spans[k].i1) + out.push_back(shape.entity); } } - // Then look for obstructions that cover the target point when expanded by r - // (i.e. if the target is not inside an object but closer than we can get to it) - - GetObstructionsInRange(filter, x-r, z-r, x+r, z+r, squares); - // Building squares are more important but returned last, so check backwards - for (std::vector::reverse_iterator it = squares.rbegin(); it != squares.rend(); ++it) - { - CFixedVector2D halfSize(it->hw + r, it->hh + r); - if (Geometry::PointIsInSquare(CFixedVector2D(it->x, it->z) - center, it->u, it->v, halfSize)) - { - square = *it; - return true; - } - } - - return false; + // TODO : Should we expand by r here? } void CCmpObstructionManager::RenderSubmit(SceneCollector& collector) diff --git a/source/simulation2/components/CCmpPathfinder.cpp b/source/simulation2/components/CCmpPathfinder.cpp index 35083b78df..ae3b94eeb9 100644 --- a/source/simulation2/components/CCmpPathfinder.cpp +++ b/source/simulation2/components/CCmpPathfinder.cpp @@ -34,8 +34,10 @@ #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/components/ICmpTerrain.h" #include "simulation2/components/ICmpWaterManager.h" +#include "simulation2/helpers/Rasterize.h" #include "simulation2/serialization/SerializeTemplates.h" + // Default cost to move a single tile is a fairly arbitrary number, which should be big // enough to be precise when multiplied/divided and small enough to never overflow when // summing the cost of a whole path. @@ -47,13 +49,14 @@ void CCmpPathfinder::Init(const CParamNode& UNUSED(paramNode)) { m_MapSize = 0; m_Grid = NULL; - m_ObstructionGrid = NULL; + m_BaseGrid = NULL; + + m_ObstructionsDirty = true; m_TerrainDirty = true; + m_NextAsyncTicket = 1; - m_DebugOverlay = NULL; - m_DebugGrid = NULL; - m_DebugPath = NULL; + m_DebugOverlay = false; m_SameTurnMovesCount = 0; @@ -90,74 +93,18 @@ void CCmpPathfinder::Init(const CParamNode& UNUSED(paramNode)) { std::string name = it->first; ENSURE((int)m_PassClasses.size() <= PASS_CLASS_BITS); - pass_class_t mask = (pass_class_t)(1u << (m_PassClasses.size() + 2)); + pass_class_t mask = PASS_CLASS_MASK_FROM_INDEX(m_PassClasses.size()); m_PassClasses.push_back(PathfinderPassability(mask, it->second)); m_PassClassMasks[name] = mask; } - - - const CParamNode::ChildrenMap& moveClasses = externalParamNode.GetChild("Pathfinder").GetChild("MovementClasses").GetChildren(); - - // First find the set of unit classes used by any terrain classes, - // and assign unique tags to terrain classes - std::set unitClassNames; - unitClassNames.insert("default"); // must always have costs for default - - { - size_t i = 0; - for (CParamNode::ChildrenMap::const_iterator it = moveClasses.begin(); it != moveClasses.end(); ++it) - { - std::string terrainClassName = it->first; - m_TerrainCostClassTags[terrainClassName] = (cost_class_t)i; - ++i; - - const CParamNode::ChildrenMap& unitClasses = it->second.GetChild("UnitClasses").GetChildren(); - for (CParamNode::ChildrenMap::const_iterator uit = unitClasses.begin(); uit != unitClasses.end(); ++uit) - unitClassNames.insert(uit->first); - } - } - - // For each terrain class, set the costs for every unit class, - // and assign unique tags to unit classes - { - size_t i = 0; - for (std::set::const_iterator nit = unitClassNames.begin(); nit != unitClassNames.end(); ++nit) - { - m_UnitCostClassTags[*nit] = (cost_class_t)i; - ++i; - - std::vector costs; - std::vector speeds; - - for (CParamNode::ChildrenMap::const_iterator it = moveClasses.begin(); it != moveClasses.end(); ++it) - { - // Default to the general costs for this terrain class - fixed cost = it->second.GetChild("@Cost").ToFixed(); - fixed speed = it->second.GetChild("@Speed").ToFixed(); - // Check for specific cost overrides for this unit class - const CParamNode& unitClass = it->second.GetChild("UnitClasses").GetChild(nit->c_str()); - if (unitClass.IsOk()) - { - cost = unitClass.GetChild("@Cost").ToFixed(); - speed = unitClass.GetChild("@Speed").ToFixed(); - } - costs.push_back((cost * DEFAULT_MOVE_COST).ToInt_RoundToZero()); - speeds.push_back(speed); - } - - m_MoveCosts.push_back(costs); - m_MoveSpeeds.push_back(speeds); - } - } } void CCmpPathfinder::Deinit() { SetDebugOverlay(false); // cleans up memory - ResetDebugPath(); - delete m_Grid; - delete m_ObstructionGrid; + SAFE_DELETE(m_Grid); + SAFE_DELETE(m_BaseGrid); } struct SerializeLongRequest @@ -170,7 +117,6 @@ struct SerializeLongRequest serialize.NumberFixed_Unbounded("z0", value.z0); SerializeGoal()(serialize, "goal", value.goal); serialize.NumberU16_Unbounded("pass class", value.passClass); - serialize.NumberU8_Unbounded("cost class", value.costClass); serialize.NumberU32_Unbounded("notify", value.notify); } }; @@ -225,7 +171,7 @@ void CCmpPathfinder::HandleMessage(const CMessage& msg, bool UNUSED(global)) case MT_WaterChanged: case MT_ObstructionMapShapeChanged: { - // TODO: we ought to only bother updating the dirtied region + // TODO PATHFINDER: we ought to only bother updating the dirtied region m_TerrainDirty = true; break; } @@ -241,20 +187,12 @@ void CCmpPathfinder::RenderSubmit(SceneCollector& collector) { for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i) collector.Submit(&m_DebugOverlayShortPathLines[i]); + + m_LongPathfinder.HierarchicalRenderSubmit(collector); } -fixed CCmpPathfinder::GetMovementSpeed(entity_pos_t x0, entity_pos_t z0, u8 costClass) -{ - UpdateGrid(); - - u16 i, j; - NearestTile(x0, z0, i, j); - TerrainTile tileTag = m_Grid->get(i, j); - return m_MoveSpeeds.at(costClass).at(GET_COST_CLASS(tileTag)); -} - -ICmpPathfinder::pass_class_t CCmpPathfinder::GetPassabilityClass(const std::string& name) +pass_class_t CCmpPathfinder::GetPassabilityClass(const std::string& name) { if (m_PassClassMasks.find(name) == m_PassClassMasks.end()) { @@ -265,43 +203,20 @@ ICmpPathfinder::pass_class_t CCmpPathfinder::GetPassabilityClass(const std::stri return m_PassClassMasks[name]; } -std::map CCmpPathfinder::GetPassabilityClasses() +std::map CCmpPathfinder::GetPassabilityClasses() { return m_PassClassMasks; } -ICmpPathfinder::cost_class_t CCmpPathfinder::GetCostClass(const std::string& name) +const PathfinderPassability* CCmpPathfinder::GetPassabilityFromMask(pass_class_t passClass) const { - if (m_UnitCostClassTags.find(name) == m_UnitCostClassTags.end()) + for (const PathfinderPassability& passability : m_PassClasses) { - LOGERROR("Invalid unit cost class name '%s'", name.c_str()); - return m_UnitCostClassTags["default"]; + if (passability.m_Mask == passClass) + return &passability; } - return m_UnitCostClassTags[name]; -} - -fixed CCmpPathfinder::DistanceToGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal) -{ - switch (goal.type) - { - case CCmpPathfinder::Goal::POINT: - return (pos - CFixedVector2D(goal.x, goal.z)).Length(); - - case CCmpPathfinder::Goal::CIRCLE: - return ((pos - CFixedVector2D(goal.x, goal.z)).Length() - goal.hw).Absolute(); - - case CCmpPathfinder::Goal::SQUARE: - { - CFixedVector2D halfSize(goal.hw, goal.hh); - CFixedVector2D d(pos.X - goal.x, pos.Y - goal.z); - return Geometry::DistanceToSquare(d, goal.u, goal.v, halfSize); - } - - default: - debug_warn(L"invalid type"); - return fixed::Zero(); - } + return NULL; } const Grid& CCmpPathfinder::GetPassabilityGrid() @@ -310,6 +225,79 @@ const Grid& CCmpPathfinder::GetPassabilityGrid() return *m_Grid; } +/** + * Given a grid of passable/impassable navcells (based on some passability mask), + * computes a new grid where a navcell is impassable (per that mask) if + * it is <=clearance navcells away from an impassable navcell in the original grid. + * The results are ORed onto the original grid. + * + * This is used for adding clearance onto terrain-based navcell passability. + * + * TODO PATHFINDER: might be nicer to get rounded corners by measuring clearances as + * Euclidean distances; currently it effectively does dist=max(dx,dy) instead. + * This would only really be a problem for big clearances. + */ +static void ExpandImpassableCells(Grid& grid, u16 clearance, pass_class_t mask) +{ + PROFILE3("ExpandImpassableCells"); + + u16 w = grid.m_W; + u16 h = grid.m_H; + + // First expand impassable cells horizontally into a temporary 1-bit grid + Grid tempGrid(w, h); + for (u16 j = 0; j < h; ++j) + { + // New cell (i,j) is blocked if (i',j) blocked for any i-clearance <= i' <= i+clearance + + // Count the number of blocked cells around i=0 + u16 numBlocked = 0; + for (u16 i = 0; i <= clearance && i < w; ++i) + if (!IS_PASSABLE(grid.get(i, j), mask)) + ++numBlocked; + + for (u16 i = 0; i < w; ++i) + { + // Store a flag if blocked by at least one nearby cell + if (numBlocked) + tempGrid.set(i, j, 1); + + // Slide the numBlocked window along: + // remove the old i-clearance value, add the new (i+1)+clearance + // (avoiding overflowing the grid) + if (i >= clearance && !IS_PASSABLE(grid.get(i-clearance, j), mask)) + --numBlocked; + if (i+1+clearance < w && !IS_PASSABLE(grid.get(i+1+clearance, j), mask)) + ++numBlocked; + } + } + + for (u16 i = 0; i < w; ++i) + { + // New cell (i,j) is blocked if (i,j') blocked for any j-clearance <= j' <= j+clearance + // Count the number of blocked cells around j=0 + u16 numBlocked = 0; + for (u16 j = 0; j <= clearance && j < h; ++j) + if (tempGrid.get(i, j)) + ++numBlocked; + + for (u16 j = 0; j < h; ++j) + { + // Add the mask if blocked by at least one nearby cell + if (numBlocked) + grid.set(i, j, grid.get(i, j) | mask); + + // Slide the numBlocked window along: + // remove the old j-clearance value, add the new (j+1)+clearance + // (avoiding overflowing the grid) + if (j >= clearance && tempGrid.get(i, j-clearance)) + --numBlocked; + if (j+1+clearance < h && tempGrid.get(i, j+1+clearance)) + ++numBlocked; + } + } +} + Grid CCmpPathfinder::ComputeShoreGrid(bool expandOnWater) { PROFILE3("ComputeShoreGrid"); @@ -323,16 +311,16 @@ Grid CCmpPathfinder::ComputeShoreGrid(bool expandOnWater) const u16 shoreMax = 32767; // First pass - find underwater tiles - Grid waterGrid(m_MapSize, m_MapSize); + Grid 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); + Pathfinding::TileCenter(i, j, x, z); bool underWater = cmpWaterManager && (cmpWaterManager->GetWaterLevel(x, z) > terrain.GetExactGroundLevelFixed(x, z)); - waterGrid.set(i, j, underWater); + waterGrid.set(i, j, underWater ? 1 : 0); } } @@ -360,7 +348,7 @@ Grid CCmpPathfinder::ComputeShoreGrid(bool expandOnWater) } } - // Expand influences to find shore distance + // Expand influences on land to find shore distance for (u16 y = 0; y < m_MapSize; ++y) { u16 min = shoreMax; @@ -381,9 +369,9 @@ Grid CCmpPathfinder::ComputeShoreGrid(bool expandOnWater) { if (!waterGrid.get(x-1, y) || expandOnWater) { - u16 g = shoreGrid.get(x - 1, y); + u16 g = shoreGrid.get(x-1, y); if (g > min) - shoreGrid.set(x - 1, y, min); + shoreGrid.set(x-1, y, min); else if (g < min) min = g; @@ -411,9 +399,9 @@ Grid CCmpPathfinder::ComputeShoreGrid(bool expandOnWater) { if (!waterGrid.get(x, y-1) || expandOnWater) { - u16 g = shoreGrid.get(x, y - 1); + u16 g = shoreGrid.get(x, y-1); if (g > min) - shoreGrid.set(x, y - 1, min); + shoreGrid.set(x, y-1, min); else if (g < min) min = g; @@ -425,17 +413,73 @@ Grid CCmpPathfinder::ComputeShoreGrid(bool expandOnWater) return shoreGrid; } +void CCmpPathfinder::ComputeTerrainPassabilityGrid(const Grid& shoreGrid) +{ + PROFILE3("terrain passability"); + + CmpPtr cmpWaterManager(GetSimContext(), SYSTEM_ENTITY); + + CTerrain& terrain = GetSimContext().GetTerrain(); + + // Compute initial terrain-dependent passability + for (int j = 0; j < m_MapSize * Pathfinding::NAVCELLS_PER_TILE; ++j) + { + for (int i = 0; i < m_MapSize * Pathfinding::NAVCELLS_PER_TILE; ++i) + { + // World-space coordinates for this navcell + fixed x, z; + Pathfinding::NavcellCenter(i, j, x, z); + + // Terrain-tile coordinates for this navcell + int itile = i / Pathfinding::NAVCELLS_PER_TILE; + int jtile = j / Pathfinding::NAVCELLS_PER_TILE; + + // Gather all the data potentially needed to determine passability: + + fixed height = terrain.GetExactGroundLevelFixed(x, z); + + fixed water; + if (cmpWaterManager) + water = cmpWaterManager->GetWaterLevel(x, z); + + fixed depth = water - height; + + //fixed slope = terrain.GetExactSlopeFixed(x, z); + // Exact slopes give kind of weird output, so just use rough tile-based slopes + fixed slope = terrain.GetSlopeFixed(itile, jtile); + + // Get world-space coordinates from shoreGrid (which uses terrain tiles) + fixed shoredist = fixed::FromInt(shoreGrid.get(itile, jtile)).MultiplyClamp(TERRAIN_TILE_SIZE); + + // Compute the passability for every class for this cell: + + NavcellData t = 0; + for (PathfinderPassability& passability : m_PassClasses) + { + if (!passability.IsPassable(depth, slope, shoredist)) + t |= passability.m_Mask; + } + + m_Grid->set(i, j, t); + } + } +} + void CCmpPathfinder::UpdateGrid() { - CmpPtr cmpTerrain(GetSystemEntity()); + PROFILE3("UpdateGrid"); + + CmpPtr cmpTerrain(GetSimContext(), SYSTEM_ENTITY); if (!cmpTerrain) return; // error + bool forceGlobalUpdate = false; + // 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); + SAFE_DELETE(m_BaseGrid); m_TerrainDirty = true; } @@ -443,127 +487,142 @@ void CCmpPathfinder::UpdateGrid() if (!m_Grid) { m_MapSize = cmpTerrain->GetTilesPerSide(); - m_Grid = new Grid(m_MapSize, m_MapSize); - m_ObstructionGrid = new Grid(m_MapSize, m_MapSize); + m_Grid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); + m_BaseGrid = new Grid(m_MapSize * Pathfinding::NAVCELLS_PER_TILE, m_MapSize * Pathfinding::NAVCELLS_PER_TILE); + forceGlobalUpdate = true; + m_TerrainDirty = true; } - CmpPtr cmpObstructionManager(GetSystemEntity()); - - bool obstructionsDirty = cmpObstructionManager->Rasterise(*m_ObstructionGrid); - - if (obstructionsDirty && !m_TerrainDirty) + CmpPtr cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY); + if (forceGlobalUpdate) { - 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 - - 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; + m_ObstructionsDirty = true; + m_ObstructionsGlobalUpdate = true; } - else if (obstructionsDirty || m_TerrainDirty) + else + m_ObstructionsDirty = cmpObstructionManager->GetDirtinessData(m_DirtinessGrid, m_ObstructionsGlobalUpdate); + + if (!m_ObstructionsDirty && !m_TerrainDirty) + return; + + // If the terrain has changed, recompute entirely m_Grid + // Else, use data from m_BaseGrid and add obstructions + if (m_TerrainDirty) { - PROFILE("UpdateGrid full"); - - // Obstructions or terrain changed - we need to recompute passability - // TODO: only bother recomputing the region that has actually changed - Grid shoreGrid = ComputeShoreGrid(); - CmpPtr cmpWaterManager(GetSystemEntity()); - CTerrain& terrain = GetSimContext().GetTerrain(); + ComputeTerrainPassabilityGrid(shoreGrid); - // Apply passability classes to terrain - for (u16 j = 0; j < m_MapSize; ++j) + // Compute off-world passability + // WARNING: CCmpRangeManager::LosIsOffWorld needs to be kept in sync with this + + const int edgeSize = 3 * Pathfinding::NAVCELLS_PER_TILE; // number of tiles around the edge that will be off-world + + NavcellData edgeMask = 0; + for (PathfinderPassability& passability : m_PassClasses) + edgeMask |= passability.m_Mask; + + int w = m_Grid->m_W; + int h = m_Grid->m_H; + + if (cmpObstructionManager->GetPassabilityCircular()) { - for (u16 i = 0; i < m_MapSize; ++i) + for (int j = 0; j < h; ++j) { - 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) + for (int i = 0; i < w; ++i) { - // 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; - } - } + // Based on CCmpRangeManager::LosIsOffWorld + // but tweaked since it's tile-based instead. + // (We double all the values so we can handle half-tile coordinates.) + // This needs to be slightly tighter than the LOS circle, + // else units might get themselves lost in the SoD around the edge. - std::string moveClass = terrain.GetMovementClass(i, j); - if (m_TerrainCostClassTags.find(moveClass) != m_TerrainCostClassTags.end()) - t |= COST_CLASS_MASK(m_TerrainCostClassTags[moveClass]); + int dist2 = (i*2 + 1 - w)*(i*2 + 1 - w) + + (j*2 + 1 - h)*(j*2 + 1 - h); - m_Grid->set(i, j, t); + if (dist2 >= (w - 2*edgeSize) * (h - 2*edgeSize)) + m_Grid->set(i, j, m_Grid->get(i, j) | edgeMask); + } } } + else + { + for (u16 j = 0; j < h; ++j) + for (u16 i = 0; i < edgeSize; ++i) + m_Grid->set(i, j, m_Grid->get(i, j) | edgeMask); + for (u16 j = 0; j < h; ++j) + for (u16 i = w-edgeSize+1; i < w; ++i) + m_Grid->set(i, j, m_Grid->get(i, j) | edgeMask); + for (u16 j = 0; j < edgeSize; ++j) + for (u16 i = edgeSize; i < w-edgeSize+1; ++i) + m_Grid->set(i, j, m_Grid->get(i, j) | edgeMask); + for (u16 j = h-edgeSize+1; j < h; ++j) + for (u16 i = edgeSize; i < w-edgeSize+1; ++i) + m_Grid->set(i, j, m_Grid->get(i, j) | edgeMask); + } + // Expand the impassability grid, for any class with non-zero clearance, + // so that we can stop units getting too close to impassable navcells + for (PathfinderPassability& passability : m_PassClasses) + { + if (!passability.m_HasClearance) + continue; + + // TODO: if multiple classes have the same clearance, we should + // only bother doing this once for them all + int clearance = (passability.m_Clearance / Pathfinding::NAVCELL_SIZE).ToInt_RoundToInfinity(); + if (clearance > 0) + ExpandImpassableCells(*m_Grid, clearance, passability.m_Mask); + } + + // Store the updated terrain-only grid + *m_BaseGrid = *m_Grid; m_TerrainDirty = false; - - ++m_Grid->m_DirtyID; } + else + { + ENSURE(m_Grid->m_W == m_BaseGrid->m_W && m_Grid->m_H == m_BaseGrid->m_H); + memcpy(m_Grid->m_Data, m_BaseGrid->m_Data, (m_Grid->m_W)*(m_Grid->m_H)*sizeof(NavcellData)); + } + + // TODO: tweak rasterizing conditions + // Add obstructions onto the grid, for any class with (possibly zero) clearance + for (PathfinderPassability& passability : m_PassClasses) + { + // TODO: if multiple classes have the same clearance, we should + // only bother running Rasterize once for them all + if (passability.m_HasClearance) + cmpObstructionManager->Rasterize(*m_Grid, passability.m_Clearance, ICmpObstructionManager::FLAG_BLOCK_PATHFINDING, passability.m_Mask); + } + + if (m_ObstructionsGlobalUpdate) + m_LongPathfinder.Reload(m_PassClassMasks, m_Grid); + else + m_LongPathfinder.Update(m_Grid, &m_DirtinessGrid); +} + +bool CCmpPathfinder::GetDirtinessData(Grid& dirtinessGrid, bool& globalUpdateNeeded) +{ + if (!m_ObstructionsDirty) + return false; + + dirtinessGrid = m_DirtinessGrid; + globalUpdateNeeded = m_ObstructionsGlobalUpdate; + return true; } ////////////////////////////////////////////////////////// // Async path requests: -u32 CCmpPathfinder::ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, entity_id_t notify) +u32 CCmpPathfinder::ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) { - AsyncLongPathRequest req = { m_NextAsyncTicket++, x0, z0, goal, passClass, costClass, notify }; + AsyncLongPathRequest req = { m_NextAsyncTicket++, x0, z0, goal, passClass, notify }; m_AsyncLongPathRequests.push_back(req); return req.ticket; } -u32 CCmpPathfinder::ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const Goal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t group, entity_id_t notify) +u32 CCmpPathfinder::ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t group, entity_id_t notify) { AsyncShortPathRequest req = { m_NextAsyncTicket++, x0, z0, r, range, goal, passClass, avoidMovingUnits, group, notify }; m_AsyncShortPathRequests.push_back(req); @@ -593,8 +652,8 @@ void CCmpPathfinder::ProcessLongRequests(const std::vector for (size_t i = 0; i < longRequests.size(); ++i) { const AsyncLongPathRequest& req = longRequests[i]; - Path path; - ComputePath(req.x0, req.z0, req.goal, req.passClass, req.costClass, path); + WaypointPath path; + ComputePath(req.x0, req.z0, req.goal, req.passClass, path); CMessagePathResult msg(req.ticket, path); GetSimContext().GetComponentManager().PostMessage(req.notify, msg); } @@ -605,7 +664,7 @@ void CCmpPathfinder::ProcessShortRequests(const std::vector cmpObstructionManager(GetSystemEntity()); @@ -687,34 +742,17 @@ ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckUnitPlacement(const IObst if (cmpObstructionManager->TestUnitShape(filter, x, z, r, NULL)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_OBSTRUCTS_FOUNDATION; - // Test against terrain: - - UpdateGrid(); - - 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; + // Test against terrain and static obstructions: + u16 i, j; + Pathfinding::NearestNavcell(x, z, i, j, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); + if (!IS_PASSABLE(m_Grid->get(i, j), passClass)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; - } - u16 i0, j0, i1, j1; - NearestTile(x - r, z - r, i0, j0); - NearestTile(x + r, z + r, i1, j1); - for (u16 j = j0; j <= j1; ++j) - { - for (u16 i = i0; i <= i1; ++i) - { - if (!IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass)) - { - return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; - } - } - } + // (Static obstructions will be redundantly tested against in both the + // obstruction-shape test and navcell-passability test, which is slightly + // inefficient but shouldn't affect behaviour) + return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; } @@ -728,7 +766,7 @@ ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckBuildingPlacement(const I 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) + entity_pos_t h, entity_id_t id, pass_class_t passClass, bool UNUSED(onlyCenterPoint)) { // Check unit obstruction CmpPtr cmpObstructionManager(GetSystemEntity()); @@ -747,38 +785,29 @@ ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckBuildingPlacement(const I if (!cmpObstruction || !cmpObstruction->GetObstructionSquare(square)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_NO_OBSTRUCTION; - if (onlyCenterPoint) + entity_pos_t expand; + const PathfinderPassability* passability = GetPassabilityFromMask(passClass); + if (passability && passability->m_HasClearance) + expand = passability->m_Clearance; + + SimRasterize::Spans spans; + SimRasterize::RasterizeRectWithClearance(spans, square, expand, Pathfinding::NAVCELL_SIZE); + for (SimRasterize::Span& span : spans) { - u16 i, j; - NearestTile(x, z, i, j); + i16 i0 = span.i0; + i16 i1 = span.i1; + i16 j = span.j; - if (IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass)) - return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; + // Fail if any span extends outside the grid + if (i0 < 0 || i1 > m_Grid->m_W || j < 0 || j > m_Grid->m_H) + return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; - 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)) - { + // Fail if any span includes an impassable tile + for (i16 i = i0; i < i1; ++i) + if (!IS_PASSABLE(m_Grid->get(i, j), passClass)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; - } - } } return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; } + diff --git a/source/simulation2/components/CCmpPathfinder_Common.h b/source/simulation2/components/CCmpPathfinder_Common.h index 831ae0cdb4..50702e3d15 100644 --- a/source/simulation2/components/CCmpPathfinder_Common.h +++ b/source/simulation2/components/CCmpPathfinder_Common.h @@ -20,11 +20,11 @@ /** * @file - * Declares CCmpPathfinder, whose implementation is split into multiple source files, - * and provides common code needed for more than one of those files. - * CCmpPathfinder includes two pathfinding algorithms (one tile-based, one vertex-based) - * with some shared state and functionality, so the code is split into - * CCmpPathfinder_Vertex.cpp, CCmpPathfinder_Tile.cpp and CCmpPathfinder.cpp + * Declares CCmpPathfinder. Its implementation is mainly done in CCmpPathfinder.cpp, + * but the short-range (vertex) pathfinding is done in CCmpPathfinder_Vertex.cpp. + * This file provides common code needed for both files. + * + * The long-range pathfinding is done by a LongPathfinder object. */ #include "simulation2/system/Component.h" @@ -34,12 +34,12 @@ #include "graphics/Overlay.h" #include "graphics/Terrain.h" #include "maths/MathUtil.h" +#include "ps/CLogger.h" +#include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/helpers/Geometry.h" -#include "simulation2/helpers/Grid.h" +#include "simulation2/helpers/LongPathfinder.h" -class PathfinderOverlay; class SceneCollector; -struct PathfindTile; #ifdef NDEBUG #define PATHFIND_DEBUG 0 @@ -47,105 +47,14 @@ struct PathfindTile; #define PATHFIND_DEBUG 1 #endif -/* - * For efficient pathfinding we want to try hard to minimise the per-tile search cost, - * so we precompute the tile passability flags and movement costs for the various different - * types of unit. - * We also want to minimise memory usage (there can easily be 100K tiles so we don't want - * to store many bytes for each). - * - * To handle passability efficiently, we have a small number of passability classes - * (e.g. "infantry", "ship"). Each unit belongs to a single passability class, and - * uses that for all its pathfinding. - * Passability is determined by water depth, terrain slope, forestness, buildingness. - * We need at least one bit per class per tile to represent passability. - * - * We use a separate bit to indicate building obstructions (instead of folding it into - * the class passabilities) so that it can be ignored when doing the accurate short paths. - * We use another bit to indicate tiles near obstructions that block construction, - * for the AI to plan safe building spots. - * - * To handle movement costs, we have an arbitrary number of unit cost classes (e.g. "infantry", "camel"), - * and a small number of terrain cost classes (e.g. "grass", "steep grass", "road", "sand"), - * and a cost mapping table between the classes (e.g. camels are fast on sand). - * We need log2(|terrain cost classes|) bits per tile to represent costs. - * - * We could have one passability bitmap per class, and another array for cost classes, - * but instead (for no particular reason) we'll pack them all into a single u16 array. - * - * We handle dynamic updates currently by recomputing the entire array, which is stupid; - * it should only bother updating the region that has changed. - */ -class PathfinderPassability -{ -public: - PathfinderPassability(ICmpPathfinder::pass_class_t mask, const CParamNode& node) : - m_Mask(mask) - { - if (node.GetChild("MinWaterDepth").IsOk()) - m_MinDepth = node.GetChild("MinWaterDepth").ToFixed(); - else - m_MinDepth = std::numeric_limits::min(); - - if (node.GetChild("MaxWaterDepth").IsOk()) - m_MaxDepth = node.GetChild("MaxWaterDepth").ToFixed(); - else - m_MaxDepth = std::numeric_limits::max(); - - if (node.GetChild("MaxTerrainSlope").IsOk()) - m_MaxSlope = node.GetChild("MaxTerrainSlope").ToFixed(); - else - m_MaxSlope = std::numeric_limits::max(); - - if (node.GetChild("MinShoreDistance").IsOk()) - m_MinShore = node.GetChild("MinShoreDistance").ToFixed(); - else - m_MinShore = std::numeric_limits::min(); - - if (node.GetChild("MaxShoreDistance").IsOk()) - m_MaxShore = node.GetChild("MaxShoreDistance").ToFixed(); - else - m_MaxShore = std::numeric_limits::max(); - - } - - bool IsPassable(fixed waterdepth, fixed steepness, fixed shoredist) - { - return ((m_MinDepth <= waterdepth && waterdepth <= m_MaxDepth) && (steepness < m_MaxSlope) && (m_MinShore <= shoredist && shoredist <= m_MaxShore)); - } - - ICmpPathfinder::pass_class_t m_Mask; -private: - fixed m_MinDepth; - fixed m_MaxDepth; - fixed m_MaxSlope; - fixed m_MinShore; - fixed m_MaxShore; -}; - -typedef u16 TerrainTile; -// 1 bit for pathfinding obstructions, -// 1 bit for construction obstructions (used by AI), -// PASS_CLASS_BITS for terrain passability (allowing PASS_CLASS_BITS classes), -// COST_CLASS_BITS for movement costs (allowing 2^COST_CLASS_BITS classes) - -const int PASS_CLASS_BITS = 10; -const int COST_CLASS_BITS = 16 - (PASS_CLASS_BITS + 2); -#define IS_TERRAIN_PASSABLE(item, classmask) (((item) & (classmask)) == 0) -#define IS_PASSABLE(item, classmask) (((item) & ((classmask) | 1)) == 0) -#define GET_COST_CLASS(item) ((item) >> (PASS_CLASS_BITS + 2)) -#define COST_CLASS_MASK(id) ( (TerrainTile) ((id) << (PASS_CLASS_BITS + 2)) ) - -typedef SparseGrid PathfindTileGrid; struct AsyncLongPathRequest { u32 ticket; entity_pos_t x0; entity_pos_t z0; - ICmpPathfinder::Goal goal; - ICmpPathfinder::pass_class_t passClass; - ICmpPathfinder::cost_class_t costClass; + PathGoal goal; + pass_class_t passClass; entity_id_t notify; }; @@ -156,8 +65,8 @@ struct AsyncShortPathRequest entity_pos_t z0; entity_pos_t r; entity_pos_t range; - ICmpPathfinder::Goal goal; - ICmpPathfinder::pass_class_t passClass; + PathGoal goal; + pass_class_t passClass; bool avoidMovingUnits; entity_id_t group; entity_id_t notify; @@ -186,11 +95,6 @@ public: std::map m_PassClassMasks; std::vector m_PassClasses; - std::map m_TerrainCostClassTags; - std::map m_UnitCostClassTags; - std::vector > m_MoveCosts; // costs[unitClass][terrainClass] - std::vector > m_MoveSpeeds; // speeds[unitClass][terrainClass] - // Dynamic state: std::vector m_AsyncLongPathRequests; @@ -201,22 +105,23 @@ public: // Lazily-constructed dynamic state (not serialized): u16 m_MapSize; // tiles per side - Grid* m_Grid; // terrain/passability information - Grid* m_ObstructionGrid; // cached obstruction information (TODO: we shouldn't bother storing this, it's redundant with LSBs of m_Grid) + Grid* m_Grid; // terrain/passability information + Grid* m_BaseGrid; // same as m_Grid, but only with terrain, to avoid some recomputations bool m_TerrainDirty; // indicates if m_Grid has been updated since terrain changed - + + // Update data, stored for the AI manager + bool m_ObstructionsDirty; + bool m_ObstructionsGlobalUpdate; + Grid m_DirtinessGrid; + + // Interface to the long-range pathfinder. + LongPathfinder m_LongPathfinder; + // For responsiveness we will process some moves in the same turn they were generated in u16 m_MaxSameTurnMoves; // max number of moves that can be created and processed in the same turn - // Debugging - output from last pathfind operation: - - PathfindTileGrid* m_DebugGrid; - u32 m_DebugSteps; - Path* m_DebugPath; - PathfinderOverlay* m_DebugOverlay; - pass_class_t m_DebugPassClass; - + bool m_DebugOverlay; std::vector m_DebugOverlayShortPathLines; static std::string GetSchema() @@ -238,35 +143,72 @@ public: virtual std::map GetPassabilityClasses(); - virtual cost_class_t GetCostClass(const std::string& name); + const PathfinderPassability* GetPassabilityFromMask(pass_class_t passClass) const; + + virtual entity_pos_t GetClearance(pass_class_t passClass) const + { + const PathfinderPassability* passability = GetPassabilityFromMask(passClass); + + if (!passability->m_HasClearance) + return fixed::Zero(); + + return passability->m_Clearance; + } + + virtual entity_pos_t GetMaximumClearance() const + { + entity_pos_t max = fixed::Zero(); + + for (const PathfinderPassability& passability : m_PassClasses) + { + if (passability.m_HasClearance && passability.m_Clearance > max) + max = passability.m_Clearance; + } + + return max; + } virtual const Grid& GetPassabilityGrid(); + virtual bool GetDirtinessData(Grid& dirtinessGrid, bool& globalUpdateNeeded); + virtual Grid ComputeShoreGrid(bool expandOnWater = false); - virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& ret); + virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) + { + m_LongPathfinder.ComputePath(x0, z0, goal, passClass, ret); + } - virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, entity_id_t notify); + virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify); - virtual void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const Goal& goal, pass_class_t passClass, Path& ret); + virtual void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret); - virtual u32 ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const Goal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t controller, entity_id_t notify); + virtual u32 ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t controller, entity_id_t notify); - virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass); + virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) + { + m_LongPathfinder.SetDebugPath(x0, z0, goal, passClass); + } - virtual void ResetDebugPath(); + virtual void SetDebugOverlay(bool enabled) + { + m_DebugOverlay = enabled; + m_LongPathfinder.SetDebugOverlay(enabled); + } - virtual void SetDebugOverlay(bool enabled); + virtual void SetHierDebugOverlay(bool enabled) + { + m_LongPathfinder.SetHierDebugOverlay(enabled, &GetSimContext()); + } - virtual fixed GetMovementSpeed(entity_pos_t x0, entity_pos_t z0, cost_class_t costClass); - - virtual CFixedVector2D GetNearestPointOnGoal(CFixedVector2D pos, const Goal& goal); + virtual void GetDebugData(u32& steps, double& time, Grid& grid) + { + m_LongPathfinder.GetDebugData(steps, time, grid); + } virtual bool CheckMovement(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, pass_class_t passClass); - virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass); - -virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint); + virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint); virtual ICmpObstruction::EFoundationCheck 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); @@ -280,30 +222,12 @@ virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionT virtual void ProcessSameTurnMoves(); - /** - * Returns the tile containing the given position - */ - void NearestTile(entity_pos_t x, entity_pos_t z, u16& i, u16& j) - { - i = (u16)clamp((x / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, m_MapSize-1); - j = (u16)clamp((z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, m_MapSize-1); - } - - /** - * Returns the position of the center of the given tile - */ - static void TileCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z) - { - x = entity_pos_t::FromInt(i*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE/2); - z = entity_pos_t::FromInt(j*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE/2); - } - - static fixed DistanceToGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal); - /** * Regenerates the grid based on the current obstruction list, if necessary */ - void UpdateGrid(); + virtual void UpdateGrid(); + + void ComputeTerrainPassabilityGrid(const Grid& shoreGrid); void RenderSubmit(SceneCollector& collector); }; diff --git a/source/simulation2/components/CCmpPathfinder_Tile.cpp b/source/simulation2/components/CCmpPathfinder_Tile.cpp deleted file mode 100644 index 21e5463a5f..0000000000 --- a/source/simulation2/components/CCmpPathfinder_Tile.cpp +++ /dev/null @@ -1,502 +0,0 @@ -/* Copyright (C) 2010 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 - * Tile-based algorithm for CCmpPathfinder. - * This is a fairly naive algorithm and could probably be improved substantially - * (hopefully without needing to change the interface much). - */ - -#include "precompiled.h" - -#include "CCmpPathfinder_Common.h" - -#include "ps/Profile.h" -#include "renderer/TerrainOverlay.h" -#include "simulation2/helpers/PriorityQueue.h" - -typedef PriorityQueueHeap, u32> PriorityQueue; - -#define PATHFIND_STATS 0 - -#define USE_DIAGONAL_MOVEMENT 1 - -// Heuristic cost to move between adjacent tiles. -// This should be similar to DEFAULT_MOVE_COST. -const u32 g_CostPerTile = 256; - -/** - * Tile data for A* computation. - * (We store an array of one of these per terrain tile, so it ought to be optimised for size) - */ -struct PathfindTile -{ -public: - enum { - STATUS_UNEXPLORED = 0, - STATUS_OPEN = 1, - STATUS_CLOSED = 2 - }; - - bool IsUnexplored() { return status == STATUS_UNEXPLORED; } - bool IsOpen() { return status == STATUS_OPEN; } - bool IsClosed() { return status == STATUS_CLOSED; } - void SetStatusOpen() { status = STATUS_OPEN; } - void SetStatusClosed() { status = STATUS_CLOSED; } - - // Get pi,pj coords of predecessor to this tile on best path, given i,j coords of this tile - u16 GetPredI(u16 i) { return (u16)(i + dpi); } - u16 GetPredJ(u16 j) { return (u16)(j + dpj); } - // Set the pi,pj coords of predecessor, given i,j coords of this tile - void SetPred(u16 pi_, u16 pj_, u16 i, u16 j) - { - dpi = (i8)((int)pi_ - (int)i); - dpj = (i8)((int)pj_ - (int)j); -#if PATHFIND_DEBUG - // predecessor must be adjacent - ENSURE(pi_-i == -1 || pi_-i == 0 || pi_-i == 1); - ENSURE(pj_-j == -1 || pj_-j == 0 || pj_-j == 1); -#endif - } - -private: - u8 status; // this only needs 2 bits - i8 dpi, dpj; // these only really need 2 bits in total -public: - u32 cost; // g (cost to this tile) - u32 h; // h (heuristic cost to goal) (TODO: is it really better for performance to store this instead of recomputing?) - -#if PATHFIND_DEBUG - u32 GetStep() { return step; } - void SetStep(u32 s) { step = s; } -private: - u32 step; // step at which this tile was last processed (for debug rendering) -#else - u32 GetStep() { return 0; } - void SetStep(u32) { } -#endif - -}; - -/** - * Terrain overlay for pathfinder debugging. - * Renders a representation of the most recent pathfinding operation. - */ -class PathfinderOverlay : public TerrainOverlay -{ - NONCOPYABLE(PathfinderOverlay); -public: - CCmpPathfinder& m_Pathfinder; - - PathfinderOverlay(CCmpPathfinder& pathfinder) - : TerrainOverlay(pathfinder.GetSimContext()), m_Pathfinder(pathfinder) - { - } - - virtual void StartRender() - { - m_Pathfinder.UpdateGrid(); - } - - virtual void ProcessTile(ssize_t i, ssize_t j) - { - if (m_Pathfinder.m_Grid && !IS_PASSABLE(m_Pathfinder.m_Grid->get((int)i, (int)j), m_Pathfinder.m_DebugPassClass)) - RenderTile(CColor(1, 0, 0, 0.6f), false); - - if (m_Pathfinder.m_DebugGrid) - { - PathfindTile& n = m_Pathfinder.m_DebugGrid->get((int)i, (int)j); - - float c = clamp((float)n.GetStep() / (float)m_Pathfinder.m_DebugSteps, 0.f, 1.f); - - if (n.IsOpen()) - RenderTile(CColor(1, 1, c, 0.6f), false); - else if (n.IsClosed()) - RenderTile(CColor(0, 1, c, 0.6f), false); - } - } - - virtual void EndRender() - { - if (m_Pathfinder.m_DebugPath) - { - std::vector& wp = m_Pathfinder.m_DebugPath->m_Waypoints; - for (size_t n = 0; n < wp.size(); ++n) - { - u16 i, j; - m_Pathfinder.NearestTile(wp[n].x, wp[n].z, i, j); - RenderTileOutline(CColor(1, 1, 1, 1), 2, false, i, j); - } - } - } -}; - -void CCmpPathfinder::SetDebugOverlay(bool enabled) -{ - if (enabled && !m_DebugOverlay) - { - m_DebugOverlay = new PathfinderOverlay(*this); - } - else if (!enabled && m_DebugOverlay) - { - delete m_DebugOverlay; - m_DebugOverlay = NULL; - } -} - -void CCmpPathfinder::SetDebugPath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass) -{ - if (!m_DebugOverlay) - return; - - delete m_DebugGrid; - m_DebugGrid = NULL; - delete m_DebugPath; - m_DebugPath = new Path(); - ComputePath(x0, z0, goal, passClass, costClass, *m_DebugPath); - m_DebugPassClass = passClass; -} - -void CCmpPathfinder::ResetDebugPath() -{ - delete m_DebugGrid; - m_DebugGrid = NULL; - delete m_DebugPath; - m_DebugPath = NULL; -} - - -////////////////////////////////////////////////////////// - - -struct PathfinderState -{ - u32 steps; // number of algorithm iterations - - u16 iGoal, jGoal; // goal tile - u16 rGoal; // radius of goal (around tile center) - - ICmpPathfinder::pass_class_t passClass; - std::vector moveCosts; - - PriorityQueue open; - // (there's no explicit closed list; it's encoded in PathfindTile) - - PathfindTileGrid* tiles; - Grid* terrain; - - bool ignoreImpassable; // allows us to escape if stuck in patches of impassability - - u32 hBest; // heuristic of closest discovered tile to goal - u16 iBest, jBest; // closest tile - -#if PATHFIND_STATS - // Performance debug counters - size_t numProcessed; - size_t numImproveOpen; - size_t numImproveClosed; - size_t numAddToOpen; - size_t sumOpenSize; -#endif -}; - -static bool AtGoal(u16 i, u16 j, const ICmpPathfinder::Goal& goal) -{ - // Allow tiles slightly more than sqrt(2) from the actual goal, - // i.e. adjacent diagonally to the target tile - fixed tolerance = entity_pos_t::FromInt(TERRAIN_TILE_SIZE*3/2); - - entity_pos_t x, z; - CCmpPathfinder::TileCenter(i, j, x, z); - fixed dist = CCmpPathfinder::DistanceToGoal(CFixedVector2D(x, z), goal); - return (dist < tolerance); -} - -// Calculate heuristic cost from tile i,j to destination -// (This ought to be an underestimate for correctness) -static u32 CalculateHeuristic(u16 i, u16 j, u16 iGoal, u16 jGoal, u16 rGoal) -{ -#if USE_DIAGONAL_MOVEMENT - CFixedVector2D pos (fixed::FromInt(i), fixed::FromInt(j)); - CFixedVector2D goal (fixed::FromInt(iGoal), fixed::FromInt(jGoal)); - fixed dist = (pos - goal).Length(); - // TODO: the heuristic could match the costs better - it's not really Euclidean movement - - fixed rdist = dist - fixed::FromInt(rGoal); - rdist = rdist.Absolute(); - - // To avoid overflows on large distances we have to convert to int before multiplying - // by the full tile cost, which means we lose some accuracy over short distances, - // so do a partial multiplication here. - // (This will overflow if sqrt(2)*tilesPerSide*premul >= 32768, so - // premul=32 means max tilesPerSide=724) - const int premul = 32; - cassert(g_CostPerTile % premul == 0); - return (rdist * premul).ToInt_RoundToZero() * (g_CostPerTile / premul); - -#else - return (abs((int)i - (int)iGoal) + abs((int)j - (int)jGoal)) * g_CostPerTile; -#endif -} - -// Calculate movement cost from predecessor tile pi,pj to tile i,j -static u32 CalculateCostDelta(u16 pi, u16 pj, u16 i, u16 j, PathfindTileGrid* tempGrid, u32 tileCost) -{ - u32 dg = tileCost; - -#if USE_DIAGONAL_MOVEMENT - // XXX: Probably a terrible hack: - // For simplicity, we only consider horizontally/vertically adjacent neighbours, but - // units can move along arbitrary lines. That results in ugly square paths, so we want - // to prefer diagonal paths. - // Instead of solving this nicely, I'll just special-case 45-degree and 30-degree lines - // by checking the three predecessor tiles (which'll be in the closed set and therefore - // likely to be reasonably stable) and reducing the cost, and use a Euclidean heuristic. - // At least this makes paths look a bit nicer for now... - - PathfindTile& p = tempGrid->get(pi, pj); - u16 ppi = p.GetPredI(pi); - u16 ppj = p.GetPredJ(pj); - if (ppi != i && ppj != j) - dg = (dg << 16) / 92682; // dg*sqrt(2)/2 - else - { - PathfindTile& pp = tempGrid->get(ppi, ppj); - int di = abs(i - pp.GetPredI(ppi)); - int dj = abs(j - pp.GetPredJ(ppj)); - if ((di == 1 && dj == 2) || (di == 2 && dj == 1)) - dg = (dg << 16) / 79742; // dg*(sqrt(5)-sqrt(2)) - } -#endif - - return dg; -} - -// Do the A* processing for a neighbour tile i,j. -static void ProcessNeighbour(u16 pi, u16 pj, u16 i, u16 j, u32 pg, PathfinderState& state) -{ -#if PATHFIND_STATS - state.numProcessed++; -#endif - - // Reject impassable tiles - TerrainTile tileTag = state.terrain->get(i, j); - if (!IS_PASSABLE(tileTag, state.passClass) && !state.ignoreImpassable) - return; - - u32 dg = CalculateCostDelta(pi, pj, i, j, state.tiles, state.moveCosts.at(GET_COST_CLASS(tileTag))); - - u32 g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor - - PathfindTile& n = state.tiles->get(i, j); - - // If this is a new tile, compute the heuristic distance - if (n.IsUnexplored()) - { - n.h = CalculateHeuristic(i, j, state.iGoal, state.jGoal, state.rGoal); - // Remember the best tile we've seen so far, in case we never actually reach the target - if (n.h < state.hBest) - { - state.hBest = n.h; - state.iBest = i; - state.jBest = j; - } - } - else - { - // 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 >= n.cost) - return; - - // Otherwise, we have a better path. - - // If we've already added this tile to the open list: - if (n.IsOpen()) - { - // This is a better path, so replace the old one with the new cost/parent - n.cost = g; - n.SetPred(pi, pj, i, j); - n.SetStep(state.steps); - state.open.promote(std::make_pair(i, j), g + n.h); -#if PATHFIND_STATS - state.numImproveOpen++; -#endif - return; - } - - // If we've already found the 'best' path to this tile: - if (n.IsClosed()) - { - // This is a better path (possible when we use inadmissible heuristics), so reopen it -#if PATHFIND_STATS - state.numImproveClosed++; -#endif - // (fall through) - } - } - - // Add it to the open list: - n.SetStatusOpen(); - n.cost = g; - n.SetPred(pi, pj, i, j); - n.SetStep(state.steps); - PriorityQueue::Item t = { std::make_pair(i, j), g + n.h }; - state.open.push(t); -#if PATHFIND_STATS - state.numAddToOpen++; -#endif -} - -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(); - - PROFILE3("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 = (u16)(goal.hw / (int)TERRAIN_TILE_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, (u16)(i-1), j, g, state); - if (i < m_MapSize-1) - ProcessNeighbour(i, j, (u16)(i+1), j, g, state); - if (j > 0) - ProcessNeighbour(i, j, i, (u16)(j-1), g, state); - if (j < m_MapSize-1) - ProcessNeighbour(i, j, i, (u16)(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; - - PROFILE2_ATTR("from: (%d, %d)", i0, j0); - PROFILE2_ATTR("to: (%d, %d)", state.iGoal, state.jGoal); - PROFILE2_ATTR("reached: (%d, %d)", state.iBest, state.jBest); - PROFILE2_ATTR("steps: %u", 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 -} diff --git a/source/simulation2/components/CCmpPathfinder_Vertex.cpp b/source/simulation2/components/CCmpPathfinder_Vertex.cpp index 27d61a76b5..544850685b 100644 --- a/source/simulation2/components/CCmpPathfinder_Vertex.cpp +++ b/source/simulation2/components/CCmpPathfinder_Vertex.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2012 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -101,14 +101,20 @@ struct Vertex }; // Obstruction edges (paths will not cross any of these). -// When used in the 'edges' list, defines the two points of the edge. -// When used in the 'edgesAA' list, defines the opposing corners of an axis-aligned square -// (from which four individual edges can be trivially computed), requiring p0 <= p1 +// Defines the two points of the edge. struct Edge { CFixedVector2D p0, p1; }; +// Axis-aligned obstruction squares (paths will not cross any of these). +// Defines the opposing corners of an axis-aligned square +// (from which four individual edges can be trivially computed), requiring p0 <= p1 +struct Square +{ + CFixedVector2D p0, p1; +}; + // Axis-aligned obstruction edges. // p0 defines one end; c1 is either the X or Y coordinate of the other end, // depending on the context in which this is used. @@ -283,209 +289,266 @@ inline static bool CheckVisibilityTop(CFixedVector2D a, CFixedVector2D b, const return true; } +typedef PriorityQueueHeap VertexPriorityQueue; -static CFixedVector2D NearestPointOnGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal) -{ - CFixedVector2D g(goal.x, goal.z); - - switch (goal.type) - { - case CCmpPathfinder::Goal::POINT: - { - return g; - } - - case CCmpPathfinder::Goal::CIRCLE: - { - CFixedVector2D d = pos - g; - if (d.IsZero()) - d = CFixedVector2D(fixed::FromInt(1), fixed::Zero()); // some arbitrary direction - d.Normalize(goal.hw); - return g + d; - } - - case CCmpPathfinder::Goal::SQUARE: - { - CFixedVector2D halfSize(goal.hw, goal.hh); - CFixedVector2D d = pos - g; - return g + Geometry::NearestPointOnSquare(d, goal.u, goal.v, halfSize); - } - - default: - debug_warn(L"invalid type"); - return CFixedVector2D(); - } -} - -CFixedVector2D CCmpPathfinder::GetNearestPointOnGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal) -{ - return NearestPointOnGoal(pos, goal); - // (It's intentional that we don't put the implementation inside this - // function, to avoid the (admittedly unmeasured and probably trivial) - // cost of a virtual call inside ComputeShortPath) -} - -typedef PriorityQueueHeap PriorityQueue; - -struct TileEdge -{ - u16 i, j; - enum { TOP, BOTTOM, LEFT, RIGHT } dir; -}; - -static void AddTerrainEdges(std::vector& edgesAA, std::vector& vertexes, - u16 i0, u16 j0, u16 i1, u16 j1, fixed r, - ICmpPathfinder::pass_class_t passClass, const Grid& terrain) +/** + * Add edges and vertexes to represent the boundaries between passable and impassable + * navcells (for impassable terrain and for static obstruction shapes). + * 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) { PROFILE("AddTerrainEdges"); - std::vector tileEdges; + // 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?) - // Find all edges between tiles of differently passability statuses - for (u16 j = j0; j <= j1; ++j) + 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 (u16 i = i0; i <= i1; ++i) + for (int i = i0; i <= i1; ++i) { - if (!IS_TERRAIN_PASSABLE(terrain.get(i, j), passClass)) + 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)) { - bool any = false; // whether we're adding any edges of this tile + 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 (j > 0 && IS_TERRAIN_PASSABLE(terrain.get(i, j-1), passClass)) + 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 + + for (int j = j0; j < j1; ++j) + { + std::vector segmentsR; + std::vector segmentsL; + + 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 { - TileEdge e = { i, j, TileEdge::BOTTOM }; - tileEdges.push_back(e); - any = true; + 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 (j < terrain.m_H-1 && IS_TERRAIN_PASSABLE(terrain.get(i, j+1), passClass)) + 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 { - TileEdge e = { i, j, TileEdge::TOP }; - tileEdges.push_back(e); - any = true; - } + 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 }); - if (i > 0 && IS_TERRAIN_PASSABLE(terrain.get(i-1, j), passClass)) - { - TileEdge e = { i, j, TileEdge::LEFT }; - tileEdges.push_back(e); - any = true; - } - - if (i < terrain.m_W-1 && IS_TERRAIN_PASSABLE(terrain.get(i+1, j), passClass)) - { - TileEdge e = { i, j, TileEdge::RIGHT }; - tileEdges.push_back(e); - any = true; - } - - // If we want to add any edge, then add the whole square to the axis-aligned-edges list. - // (The inner edges are redundant but it's easier than trying to split the squares apart.) - if (any) - { - CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r); - CFixedVector2D v1 = CFixedVector2D(fixed::FromInt((i+1) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r); - Edge e = { v0, v1 }; - edgesAA.push_back(e); + ia = segmentsL[n]; + ib = ia + 1; } } } } - // TODO: maybe we should precompute these terrain edges since they'll rarely change? - - // TODO: for efficiency (minimising the A* search space), we should coalesce adjoining edges - - // Add all the tile outer edges to the search vertex lists - for (size_t n = 0; n < tileEdges.size(); ++n) + for (int i = i0; i < i1; ++i) { - u16 i = tileEdges[n].i; - u16 j = tileEdges[n].j; - CFixedVector2D v0, v1; - Vertex vert; - vert.status = Vertex::UNEXPLORED; - vert.quadOutward = QUADRANT_ALL; + std::vector segmentsU; + std::vector segmentsD; - switch (tileEdges[n].dir) + for (int j = j0; j <= j1; ++j) { - case TileEdge::BOTTOM: - { - v0 = CFixedVector2D(fixed::FromInt(i * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r); - v1 = CFixedVector2D(fixed::FromInt((i+1) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r); - vert.p.X = v0.X - EDGE_EXPAND_DELTA; vert.p.Y = v0.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TR; vertexes.push_back(vert); - vert.p.X = v1.X + EDGE_EXPAND_DELTA; vert.p.Y = v1.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TL; vertexes.push_back(vert); - break; + 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); } - case TileEdge::TOP: + + if (!segmentsU.empty()) { - v0 = CFixedVector2D(fixed::FromInt((i+1) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r); - v1 = CFixedVector2D(fixed::FromInt(i * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r); - vert.p.X = v0.X + EDGE_EXPAND_DELTA; vert.p.Y = v0.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BL; vertexes.push_back(vert); - vert.p.X = v1.X - EDGE_EXPAND_DELTA; vert.p.Y = v1.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BR; vertexes.push_back(vert); - break; + 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; + } + } } - case TileEdge::LEFT: + + if (!segmentsD.empty()) { - v0 = CFixedVector2D(fixed::FromInt(i * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r); - v1 = CFixedVector2D(fixed::FromInt(i * (int)TERRAIN_TILE_SIZE) - r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r); - vert.p.X = v0.X - EDGE_EXPAND_DELTA; vert.p.Y = v0.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BR; vertexes.push_back(vert); - vert.p.X = v1.X - EDGE_EXPAND_DELTA; vert.p.Y = v1.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TR; vertexes.push_back(vert); - break; - } - case TileEdge::RIGHT: - { - v0 = CFixedVector2D(fixed::FromInt((i+1) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt(j * (int)TERRAIN_TILE_SIZE) - r); - v1 = CFixedVector2D(fixed::FromInt((i+1) * (int)TERRAIN_TILE_SIZE) + r, fixed::FromInt((j+1) * (int)TERRAIN_TILE_SIZE) + r); - vert.p.X = v0.X + EDGE_EXPAND_DELTA; vert.p.Y = v0.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TL; vertexes.push_back(vert); - vert.p.X = v1.X + EDGE_EXPAND_DELTA; vert.p.Y = v1.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BL; vertexes.push_back(vert); - break; - } + 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(CFixedVector2D a, - const std::vector& edgesAA, + const std::vector& edges, + const std::vector& squares, + std::vector& edgesUnaligned, std::vector& edgesLeft, std::vector& edgesRight, std::vector& edgesBottom, std::vector& edgesTop) { - edgesLeft.reserve(edgesAA.size()); - edgesRight.reserve(edgesAA.size()); - edgesBottom.reserve(edgesAA.size()); - edgesTop.reserve(edgesAA.size()); + edgesLeft.reserve(squares.size()); + edgesRight.reserve(squares.size()); + edgesBottom.reserve(squares.size()); + edgesTop.reserve(squares.size()); - for (size_t i = 0; i < edgesAA.size(); ++i) + for (const Square& square : squares) { - if (a.X <= edgesAA[i].p0.X) + 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) { - EdgeAA e = { edgesAA[i].p0, edgesAA[i].p1.Y }; - edgesLeft.push_back(e); + 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 }); + } } - if (a.X >= edgesAA[i].p1.X) + else if (edge.p0.Y == edge.p1.Y) { - EdgeAA e = { edgesAA[i].p1, edgesAA[i].p0.Y }; - edgesRight.push_back(e); - } - if (a.Y <= edgesAA[i].p0.Y) - { - EdgeAA e = { edgesAA[i].p0, edgesAA[i].p1.X }; - edgesBottom.push_back(e); - } - if (a.Y >= edgesAA[i].p1.Y) - { - EdgeAA e = { edgesAA[i].p1, edgesAA[i].p0.X }; - edgesTop.push_back(e); + 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 edges by approximate proximity to a fixed point. + * Functor for sorting edge-squares by approximate proximity to a fixed point. */ -struct EdgeSort +struct SquareSort { CFixedVector2D src; - EdgeSort(CFixedVector2D src) : src(src) { } - bool operator()(const Edge& a, const Edge& b) + SquareSort(CFixedVector2D src) : src(src) { } + bool operator()(const Square& a, const Square& b) { if ((a.p0 - src).CompareLength(b.p0 - src) < 0) return true; @@ -495,12 +558,9 @@ struct EdgeSort void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t r, - entity_pos_t range, const Goal& goal, pass_class_t passClass, Path& path) + entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& path) { - UpdateGrid(); // TODO: only need to bother updating if the terrain changed - PROFILE3("ComputeShortPath"); -// ScopeTimer UID__(L"ComputeShortPath"); m_DebugOverlayShortPathLines.clear(); @@ -511,17 +571,19 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1); switch (goal.type) { - case CCmpPathfinder::Goal::POINT: + case PathGoal::POINT: { SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true); break; } - case CCmpPathfinder::Goal::CIRCLE: + case PathGoal::CIRCLE: + case PathGoal::INVERTED_CIRCLE: { SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true); break; } - case CCmpPathfinder::Goal::SQUARE: + case PathGoal::SQUARE: + case PathGoal::INVERTED_SQUARE: { float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat()); SimRender::ConstructSquareOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true); @@ -533,7 +595,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // List of collision edges - paths must never cross these. // (Edges are one-sided so intersections are fine in one direction, but not the other direction.) std::vector edges; - std::vector edgesAA; // axis-aligned squares + std::vector edgeSquares; // axis-aligned squares; equivalent to 4 edges // Create impassable edges at the max-range boundary, so we can't escape the region // where we're meant to be searching @@ -541,17 +603,13 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, fixed rangeXMax = x0 + range; fixed rangeZMin = z0 - range; fixed rangeZMax = z0 + range; - { - // (The edges are the opposite direction to usual, so it's an inside-out square) - Edge e0 = { CFixedVector2D(rangeXMin, rangeZMin), CFixedVector2D(rangeXMin, rangeZMax) }; - Edge e1 = { CFixedVector2D(rangeXMin, rangeZMax), CFixedVector2D(rangeXMax, rangeZMax) }; - Edge e2 = { CFixedVector2D(rangeXMax, rangeZMax), CFixedVector2D(rangeXMax, rangeZMin) }; - Edge e3 = { CFixedVector2D(rangeXMax, rangeZMin), CFixedVector2D(rangeXMin, rangeZMin) }; - edges.push_back(e0); - edges.push_back(e1); - edges.push_back(e2); - edges.push_back(e3); - } + + // (The edges are the opposite direction to usual, so it's an inside-out square) + edges.emplace_back(Edge{ CFixedVector2D(rangeXMin, rangeZMin), CFixedVector2D(rangeXMin, rangeZMax) }); + edges.emplace_back(Edge{ CFixedVector2D(rangeXMin, rangeZMax), CFixedVector2D(rangeXMax, rangeZMax) }); + edges.emplace_back(Edge{ CFixedVector2D(rangeXMax, rangeZMax), CFixedVector2D(rangeXMax, rangeZMin) }); + edges.emplace_back(Edge{ CFixedVector2D(rangeXMax, rangeZMin), CFixedVector2D(rangeXMin, rangeZMin) }); + // List of obstruction vertexes (plus start/end points); we'll try to find paths through // the graph defined by these vertexes @@ -559,7 +617,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // Add the start point to the graph CFixedVector2D posStart(x0, z0); - fixed hStart = (posStart - NearestPointOnGoal(posStart, goal)).Length(); + fixed hStart = (posStart - goal.NearestPointOnGoal(posStart)).Length(); Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL }; vertexes.push_back(start); const size_t START_VERTEX_ID = 0; @@ -574,9 +632,9 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // Add terrain obstructions { u16 i0, j0, i1, j1; - NearestTile(rangeXMin, rangeZMin, i0, j0); - NearestTile(rangeXMax, rangeZMax, i1, j1); - AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid); + 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(edges, vertexes, i0, j0, i1, j1, passClass, *m_Grid); } // Find all the obstruction squares that might affect us @@ -586,7 +644,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // Resize arrays to reduce reallocations vertexes.reserve(vertexes.size() + squares.size()*4); - edgesAA.reserve(edgesAA.size() + squares.size()); // (assume most squares are AA) + edgeSquares.reserve(edgeSquares.size() + squares.size()); // (assume most squares are AA) // Convert each obstruction square into collision edges and search graph vertexes for (size_t i = 0; i < squares.size(); ++i) @@ -615,7 +673,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // Add the edges: - CFixedVector2D h0(squares[i].hw + r, squares[i].hh + r); + CFixedVector2D h0(squares[i].hw + r, squares[i].hh + r); CFixedVector2D h1(squares[i].hw + r, -(squares[i].hh + r)); CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v)); @@ -623,20 +681,13 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v)); CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v)); if (aa) - { - Edge e = { ev1, ev3 }; - edgesAA.push_back(e); - } + edgeSquares.emplace_back(Square{ ev1, ev3 }); else { - Edge e0 = { ev0, ev1 }; - Edge e1 = { ev1, ev2 }; - Edge e2 = { ev2, ev3 }; - Edge e3 = { ev3, ev0 }; - edges.push_back(e0); - edges.push_back(e1); - edges.push_back(e2); - edges.push_back(e3); + edges.emplace_back(Edge{ ev0, ev1 }); + edges.emplace_back(Edge{ ev1, ev2 }); + edges.emplace_back(Edge{ ev2, ev3 }); + edges.emplace_back(Edge{ ev3, ev0 }); } // TODO: should clip out vertexes and edges that are outside the range, @@ -645,36 +696,75 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, ENSURE(vertexes.size() < 65536); // we store array indexes as u16 + // Render the debug overlay if (m_DebugOverlay) { - // Render the obstruction edges - for (size_t i = 0; i < edges.size(); ++i) +#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.push_back(SOverlayLine()); - m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); - std::vector xz; - xz.push_back(edges[i].p0.X.ToFloat()); - xz.push_back(edges[i].p0.Y.ToFloat()); - xz.push_back(edges[i].p1.X.ToFloat()); - xz.push_back(edges[i].p1.Y.ToFloat()); - SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); + 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(GetSimContext(), x, z, 0.5f, + m_DebugOverlayShortPathLines.back(), true); + else + SimRender::ConstructClosedArcOnGround(GetSimContext(), x, z, 0.5f, + a0 * ((float)M_PI*2.0f), a1 * ((float)M_PI*2.0f), + m_DebugOverlayShortPathLines.back(), true); } - for (size_t i = 0; i < edgesAA.size(); ++i) + // 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(GetSimContext(), 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; - xz.push_back(edgesAA[i].p0.X.ToFloat()); - xz.push_back(edgesAA[i].p0.Y.ToFloat()); - xz.push_back(edgesAA[i].p0.X.ToFloat()); - xz.push_back(edgesAA[i].p1.Y.ToFloat()); - xz.push_back(edgesAA[i].p1.X.ToFloat()); - xz.push_back(edgesAA[i].p1.Y.ToFloat()); - xz.push_back(edgesAA[i].p1.X.ToFloat()); - xz.push_back(edgesAA[i].p0.Y.ToFloat()); - xz.push_back(edgesAA[i].p0.X.ToFloat()); - xz.push_back(edgesAA[i].p0.Y.ToFloat()); + 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(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); } } @@ -690,10 +780,10 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked // as closed), we won't be doing any redundant visibility computations. - PROFILE_START("A*"); + PROFILE_START("Short pathfinding - A*"); - PriorityQueue open; - PriorityQueue::Item qiStart = { START_VERTEX_ID, start.h }; + VertexPriorityQueue open; + VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h }; open.push(qiStart); u16 idBest = START_VERTEX_ID; @@ -702,7 +792,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, while (!open.empty()) { // Move best tile from open to closed - PriorityQueue::Item curr = open.pop(); + VertexPriorityQueue::Item curr = open.pop(); vertexes[curr.id].status = Vertex::CLOSED; // If we've reached the destination, stop @@ -714,13 +804,14 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // Sort the edges so ones nearer this vertex are checked first by CheckVisibility, // since they're more likely to block the rays - std::sort(edgesAA.begin(), edgesAA.end(), EdgeSort(vertexes[curr.id].p)); + std::sort(edgeSquares.begin(), edgeSquares.end(), SquareSort(vertexes[curr.id].p)); + std::vector edgesUnaligned; std::vector edgesLeft; std::vector edgesRight; std::vector edgesBottom; std::vector edgesTop; - SplitAAEdges(vertexes[curr.id].p, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop); + SplitAAEdges(vertexes[curr.id].p, edges, edgeSquares, edgesUnaligned, edgesLeft, edgesRight, edgesBottom, edgesTop); // Check the lines to every other vertex for (size_t n = 0; n < vertexes.size(); ++n) @@ -732,7 +823,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, CFixedVector2D npos; if (n == GOAL_VERTEX_ID) { - npos = NearestPointOnGoal(vertexes[curr.id].p, goal); + npos = goal.NearestPointOnGoal(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 @@ -768,7 +859,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) && CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) && CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) && - CheckVisibility(vertexes[curr.id].p, npos, edges); + CheckVisibility(vertexes[curr.id].p, npos, edgesUnaligned); /* // Render the edges that we examine @@ -792,7 +883,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // Add it to the open list: vertexes[n].status = Vertex::OPEN; vertexes[n].g = g; - vertexes[n].h = DistanceToGoal(npos, goal); + vertexes[n].h = goal.DistanceToPoint(npos); vertexes[n].pred = curr.id; // If this is an axis-aligned shape, the path must continue in the same quadrant @@ -805,7 +896,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, if (n == GOAL_VERTEX_ID) vertexes[n].p = npos; // remember the new best goal position - PriorityQueue::Item t = { (u16)n, g + vertexes[n].h }; + VertexPriorityQueue::Item t = { (u16)n, g + vertexes[n].h, vertexes[n].h }; open.push(t); // Remember the heuristically best vertex we've seen so far, in case we never actually reach the target @@ -823,6 +914,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, continue; // Otherwise, we have a better path, so replace the old one with the new cost/parent + fixed gprev = vertexes[n].g; vertexes[n].g = g; vertexes[n].pred = curr.id; @@ -834,7 +926,7 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, if (n == GOAL_VERTEX_ID) vertexes[n].p = npos; // remember the new best goal position - open.promote((u16)n, g + vertexes[n].h); + open.promote((u16)n, gprev + vertexes[n].h, g + vertexes[n].h, vertexes[n].h); } } } @@ -842,18 +934,17 @@ void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, // Reconstruct the path (in reverse) for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred) - { - Waypoint w = { vertexes[id].p.X, vertexes[id].p.Y }; - path.m_Waypoints.push_back(w); - } + path.m_Waypoints.emplace_back(Waypoint{ vertexes[id].p.X, vertexes[id].p.Y }); - PROFILE_END("A*"); + PROFILE_END("Short pathfinding - A*"); } bool CCmpPathfinder::CheckMovement(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, pass_class_t passClass) { + // Test against dynamic obstructions first + CmpPtr cmpObstructionManager(GetSystemEntity()); if (!cmpObstructionManager) return false; @@ -861,34 +952,7 @@ bool CCmpPathfinder::CheckMovement(const IObstructionTestFilter& filter, if (cmpObstructionManager->TestLine(filter, x0, z0, x1, z1, r)) return false; - // Test against terrain: - - // (TODO: this could probably be a tiny bit faster by not reusing all the vertex computation code) - - UpdateGrid(); - - std::vector edgesAA; - std::vector vertexes; - - u16 i0, j0, i1, j1; - NearestTile(std::min(x0, x1) - r, std::min(z0, z1) - r, i0, j0); - NearestTile(std::max(x0, x1) + r, std::max(z0, z1) + r, i1, j1); - AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid); - - CFixedVector2D a(x0, z0); - CFixedVector2D b(x1, z1); - - std::vector edgesLeft; - std::vector edgesRight; - std::vector edgesBottom; - std::vector edgesTop; - SplitAAEdges(a, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop); - - bool visible = - CheckVisibilityLeft(a, b, edgesLeft) && - CheckVisibilityRight(a, b, edgesRight) && - CheckVisibilityBottom(a, b, edgesBottom) && - CheckVisibilityTop(a, b, edgesTop); - - return visible; + // Test against the passability grid. + // This ignores r. + return m_LongPathfinder.CheckLineMovement(x0, z0, x1, z1, passClass); } diff --git a/source/simulation2/components/CCmpRallyPointRenderer.cpp b/source/simulation2/components/CCmpRallyPointRenderer.cpp index a59d8c8a9f..743e9ea841 100644 --- a/source/simulation2/components/CCmpRallyPointRenderer.cpp +++ b/source/simulation2/components/CCmpRallyPointRenderer.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2014 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -68,9 +68,8 @@ struct SVisibilitySegment class CCmpRallyPointRenderer : public ICmpRallyPointRenderer { // import some types for less verbosity - typedef ICmpPathfinder::Path Path; - typedef ICmpPathfinder::Goal Goal; - typedef ICmpPathfinder::Waypoint Waypoint; + typedef WaypointPath Path; + typedef PathGoal Goal; typedef ICmpRangeManager::CLosQuerier CLosQuerier; typedef SOverlayTexturedLine::LineCapType LineCapType; @@ -114,7 +113,6 @@ protected: std::wstring m_LineTexturePath; std::wstring m_LineTextureMaskPath; std::string m_LinePassabilityClass; ///< Pathfinder passability class to use for computing the (long-range) marker line path. - std::string m_LineCostClass; ///< Pathfinder cost class to use for computing the (long-range) marker line path. CTexturePtr m_Texture; CTexturePtr m_TextureMask; @@ -140,7 +138,6 @@ public: "square" "" "" - "default" "default" "" "" @@ -195,9 +192,6 @@ public: "" "" "" - "" - "" - "" ""; } @@ -412,6 +406,11 @@ private: */ void FixFootprintWaypoints(std::vector& coords, CmpPtr cmpPosition, CmpPtr cmpFootprint); + /** + * Get the point on the footprint edge that's as close from "start" as possible. + */ + void GetClosestsEdgePointFrom(CFixedVector2D& result, CFixedVector2D& start, CmpPtr cmpPosition, CmpPtr cmpFootprint); + /** * Returns a list of indices of waypoints in the current path (m_Path[index]) where the LOS visibility changes, ordered from * building/previous rally point to rally point. Used to construct the overlay line segments and track changes to the SoD. @@ -477,7 +476,6 @@ void CCmpRallyPointRenderer::Init(const CParamNode& paramNode) m_LineTextureMaskPath = paramNode.GetChild("LineTextureMask").ToString(); m_LineStartCapType = SOverlayTexturedLine::StrToLineCapType(paramNode.GetChild("LineStartCap").ToString()); m_LineEndCapType = SOverlayTexturedLine::StrToLineCapType(paramNode.GetChild("LineEndCap").ToString()); - m_LineCostClass = paramNode.GetChild("LineCostClass").ToUTF8(); m_LinePassabilityClass = paramNode.GetChild("LinePassabilityClass").ToUTF8(); // --------------------------------------------------------------------------------------------- @@ -621,66 +619,61 @@ void CCmpRallyPointRenderer::RecomputeRallyPointPath(size_t index, CmpPtrGetPosition2D().X; - pathStartY = cmpPosition->GetPosition2D().Y; - } - else - { - pathStartX = m_RallyPoints[index-1].X; - pathStartY = m_RallyPoints[index-1].Y; - } - // Find a long path to the goal point -- this uses the tile-based pathfinder, which will return a - // list of waypoints (i.e. a Path) from the building/previous rally point to the goal, where each + // list of waypoints (i.e. a Path) from the goal to the foundation/previous rally point, where each // waypoint is centered at a tile. We'll have to do some post-processing on the path to get it smooth. Path path; std::vector& waypoints = path.m_Waypoints; + CFixedVector2D start(cmpPosition->GetPosition2D()); Goal goal = { Goal::POINT, m_RallyPoints[index].X, m_RallyPoints[index].Y }; - cmpPathfinder->ComputePath( - pathStartX, - pathStartY, - goal, - cmpPathfinder->GetPassabilityClass(m_LinePassabilityClass), - cmpPathfinder->GetCostClass(m_LineCostClass), - path - ); + + if (index == 0) + GetClosestsEdgePointFrom(start,m_RallyPoints[index], cmpPosition, cmpFootprint); + else + { + start.X = m_RallyPoints[index-1].X; + start.Y = m_RallyPoints[index-1].Y; + } + cmpPathfinder->ComputePath(start.X, start.Y, goal, cmpPathfinder->GetPassabilityClass(m_LinePassabilityClass), path); // Check if we got a path back; if not we probably have two markers less than one tile apart. if (path.m_Waypoints.size() < 2) { - m_Path[index].push_back(CVector2D(goal.x.ToFloat(), goal.z.ToFloat())); - m_Path[index].push_back(CVector2D(pathStartX.ToFloat(), pathStartY.ToFloat())); + m_Path[index].emplace_back(start.X.ToFloat(), start.Y.ToFloat()); + m_Path[index].emplace_back(m_RallyPoints[index].X.ToFloat(), m_RallyPoints[index].Y.ToFloat()); return; } + else if (index == 0) + { + // sometimes this ends up not being optimal if you asked for a long path, so improve. + CFixedVector2D newend(waypoints[waypoints.size()-2].x,waypoints[waypoints.size()-2].z); + GetClosestsEdgePointFrom(newend,newend, cmpPosition, cmpFootprint); + waypoints.back().x = newend.X; + waypoints.back().z = newend.Y; + } + else + { + // make sure we actually start at the rallypoint because the pathfinder moves us to a usable tile. + waypoints.back().x = m_RallyPoints[index-1].X; + waypoints.back().z = m_RallyPoints[index-1].Y; + } + + // pathfinder makes us go to the nearest passable cell which isn't always what we want + waypoints[0].x = m_RallyPoints[index].X; + waypoints[0].z = m_RallyPoints[index].Y; // From here on, we choose to represent the waypoints as CVector2D floats to avoid to have to convert back and forth // between fixed-point Waypoint/CFixedVector2D and various other float-based formats used by interpolation and whatnot. // Since we'll only be further using these points for rendering purposes, using floats should be fine. - // Make sure to add the actual goal point as the last point (the long pathfinder only finds paths to the tile closest to the - // goal, so we need to complete the last bit from the closest tile to the rally point itself) - // NOTE: the points are returned in reverse order (from the goal to the start point), so we actually need to insert it at the - // front of the coordinate list. Hence, we'll do this first before appending the rest of the fixed waypoints as CVector2Ds. - - Waypoint& lastWaypoint = waypoints.back(); - if (lastWaypoint.x != goal.x || lastWaypoint.z != goal.z) - m_Path[index].push_back(CVector2D(goal.x.ToFloat(), goal.z.ToFloat())); - - // add the rest of the waypoints - for (size_t i = 0; i < waypoints.size(); ++i) - m_Path[index].push_back(CVector2D(waypoints[i].x.ToFloat(), waypoints[i].z.ToFloat())); + for (Waypoint& waypoint : waypoints) + m_Path[index].emplace_back(waypoint.x.ToFloat(), waypoint.z.ToFloat()); // add the start position - m_Path[index].push_back(CVector2D(pathStartX.ToFloat(), pathStartY.ToFloat())); + // m_Path[index].emplace_back(m_RallyPoints[index].X.ToFloat(), m_RallyPoints[index].Y.ToFloat()); - // ------------------------------------------------------------------------------------------- - // post-processing + // Post-processing // Linearize the path; // Pass through the waypoints, averaging each waypoint with its next one except the last one. Because the path @@ -691,45 +684,40 @@ void CCmpRallyPointRenderer::RecomputeRallyPointPath(size_t index, CmpPtr /////////////////////////////////////////////// + // Debug overlays if (m_EnableDebugNodeOverlay) { while (index >= m_DebugNodeOverlays.size()) - { - std::vector tmp; - m_DebugNodeOverlays.push_back(tmp); - } + m_DebugNodeOverlays.emplace_back(); + m_DebugNodeOverlays[index].clear(); } if (m_EnableDebugNodeOverlay && m_SmoothPath) { // Create separate control point overlays so we can differentiate when using smoothing (offset them a little higher from the // terrain so we can still see them after the interpolated points are added) - for (size_t j = 0; j < m_Path[index].size(); ++j) + for (CVector2D& point : m_Path[index]) { SOverlayLine overlayLine; overlayLine.m_Color = CColor(1.0f, 0.0f, 0.0f, 1.0f); overlayLine.m_Thickness = 2; - SimRender::ConstructSquareOnGround(GetSimContext(), m_Path[index][j].X, m_Path[index][j].Y, 0.2f, 0.2f, 1.0f, overlayLine, true); + SimRender::ConstructSquareOnGround(GetSimContext(), point.X, point.Y, 0.2f, 0.2f, 1.0f, overlayLine, true); m_DebugNodeOverlays[index].push_back(overlayLine); } } - //// ////////////////////////////////////////////// if (m_SmoothPath) // The number of points to interpolate goes hand in hand with the maximum amount of node links allowed to be joined together // by the visibility reduction. The more node links that can be joined together, the more interpolated points you need to // generate to be able to deal with local terrain height changes. - SimRender::InterpolatePointsRNS(m_Path[index], false, 0, 8); // no offset, keep line at its exact path - - // ------------------------------------------------------------------------------------------- + SimRender::InterpolatePointsRNS(m_Path[index], false, 0, 4); // no offset, keep line at its exact path // find which point is the last visible point before going into the SoD, so we have a point to compare to on the next turn GetVisibilitySegments(m_VisibilitySegments[index], index); @@ -932,6 +920,53 @@ void CCmpRallyPointRenderer::UpdateOverlayLines() } } +void CCmpRallyPointRenderer::GetClosestsEdgePointFrom(CFixedVector2D& result, CFixedVector2D& start, CmpPtr cmpPosition, CmpPtr cmpFootprint) +{ + ENSURE(cmpPosition); + ENSURE(cmpFootprint); + + // grab the shape and dimensions of the footprint + entity_pos_t footprintSize0, footprintSize1, footprintHeight; + ICmpFootprint::EShape footprintShape; + cmpFootprint->GetShape(footprintShape, footprintSize0, footprintSize1, footprintHeight); + + // grab the center of the footprint + CFixedVector2D center = cmpPosition->GetPosition2D(); + + switch (footprintShape) + { + case ICmpFootprint::SQUARE: + { + // in this case, footprintSize0 and 1 indicate the size along the X and Z axes, respectively. + + // the building's footprint could be rotated any which way, so let's get the rotation around the Y axis + // and the rotated unit vectors in the X/Z plane of the shape's footprint + // (the Footprint itself holds only the outline, the Position holds the orientation) + + fixed s, c; // sine and cosine of the Y axis rotation angle (aka the yaw) + fixed a = cmpPosition->GetRotation().Y; + sincos_approx(a, s, c); + CFixedVector2D u(c, -s); // unit vector along the rotated X axis + CFixedVector2D v(s, c); // unit vector along the rotated Z axis + CFixedVector2D halfSize(footprintSize0/2, footprintSize1/2); + + CFixedVector2D footprintEdgePoint = Geometry::NearestPointOnSquare(start - center, u, v, halfSize); + result = center + footprintEdgePoint; + break; + } + case ICmpFootprint::CIRCLE: + { + // in this case, both footprintSize0 and 1 indicate the circle's radius + // Transform target to the point nearest on the edge. + CFixedVector2D centerVec2D(center.X, center.Y); + CFixedVector2D centerToLast(start - centerVec2D); + centerToLast.Normalize(); + result = centerVec2D + (centerToLast.Multiply(footprintSize0)); + break; + } + } +} + void CCmpRallyPointRenderer::FixFootprintWaypoints(std::vector& coords, CmpPtr cmpPosition, CmpPtr cmpFootprint) { ENSURE(cmpPosition); @@ -1034,7 +1069,7 @@ void CCmpRallyPointRenderer::ReduceSegmentsByVisibility(std::vector& std::vector newCoords; StationaryOnlyObstructionFilter obstructionFilter; entity_pos_t lineRadius = fixed::FromFloat(m_LineThickness); - ICmpPathfinder::pass_class_t passabilityClass = cmpPathFinder->GetPassabilityClass(m_LinePassabilityClass); + pass_class_t passabilityClass = cmpPathFinder->GetPassabilityClass(m_LinePassabilityClass); newCoords.push_back(coords[0]); // save the first base node diff --git a/source/simulation2/components/CCmpRangeManager.cpp b/source/simulation2/components/CCmpRangeManager.cpp index a8778232a3..34bd919e34 100644 --- a/source/simulation2/components/CCmpRangeManager.cpp +++ b/source/simulation2/components/CCmpRangeManager.cpp @@ -27,6 +27,7 @@ #include "simulation2/components/ICmpMirage.h" #include "simulation2/components/ICmpOwnership.h" #include "simulation2/components/ICmpPosition.h" +#include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/components/ICmpTerritoryManager.h" #include "simulation2/components/ICmpVisibility.h" #include "simulation2/components/ICmpVision.h" @@ -1734,13 +1735,25 @@ public: virtual void ExploreTerritories() { + PROFILE3("ExploreTerritories"); + CmpPtr cmpTerritoryManager(GetSystemEntity()); - const Grid& grid = cmpTerritoryManager->GetTerritoryGrid(); - ENSURE(grid.m_W == m_TerrainVerticesPerSide-1 && grid.m_H == m_TerrainVerticesPerSide-1); - // For each tile, if it is owned by a valid player then update the LOS - // for every vertex around that tile, to mark them as explored + // Territory data is stored per territory-tile (typically a multiple of terrain-tiles). + // LOS data is stored per terrain-tile vertex. + + // For each territory-tile, if it is owned by a valid player then update the LOS + // for every vertex inside/around that tile, to mark them as explored. + + // Currently this code doesn't support territory-tiles smaller than terrain-tiles + // (it will get scale==0 and break), or a non-integer multiple, so check that first + cassert(ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE >= Pathfinding::NAVCELLS_PER_TILE); + cassert(ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE % Pathfinding::NAVCELLS_PER_TILE == 0); + + int scale = ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE / Pathfinding::NAVCELLS_PER_TILE; + + ENSURE(grid.m_W*scale == m_TerrainVerticesPerSide-1 && grid.m_H*scale == m_TerrainVerticesPerSide-1); for (u16 j = 0; j < grid.m_H; ++j) { @@ -1749,15 +1762,19 @@ public: u8 p = grid.get(i, j) & ICmpTerritoryManager::TERRITORY_PLAYER_MASK; if (p > 0 && p <= MAX_LOS_PLAYER_ID) { - u32 &explored = m_ExploredVertices.at(p); - explored += !(m_LosState[i + j*m_TerrainVerticesPerSide] & (LOS_EXPLORED << (2*(p-1)))); - m_LosState[i + j*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1))); - explored += !(m_LosState[i+1 + j*m_TerrainVerticesPerSide] & (LOS_EXPLORED << (2*(p-1)))); - m_LosState[i+1 + j*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1))); - explored += !(m_LosState[i + (j+1)*m_TerrainVerticesPerSide] & (LOS_EXPLORED << (2*(p-1)))); - m_LosState[i + (j+1)*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1))); - explored += !(m_LosState[i+1 + (j+1)*m_TerrainVerticesPerSide] & (LOS_EXPLORED << (2*(p-1)))); - m_LosState[i+1 + (j+1)*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1))); + u32& explored = m_ExploredVertices.at(p); + for (int dj = 0; dj <= scale; ++dj) + { + for (int di = 0; di <= scale; ++di) + { + u32& losState = m_LosState[(i*scale+di) + (j*scale+dj)*m_TerrainVerticesPerSide]; + if (!(losState & (LOS_EXPLORED << (2*(p-1))))) + { + explored++; + losState |= (LOS_EXPLORED << (2*(p-1))); + } + } + } } } } @@ -1846,7 +1863,7 @@ public: */ inline bool LosIsOffWorld(ssize_t i, ssize_t j) { - // WARNING: CCmpObstructionManager::Rasterise needs to be kept in sync with this + // WARNING: CCmpPathfinder::UpdateGrid needs to be kept in sync with this const ssize_t edgeSize = 3; // number of vertexes around the edge that will be off-world if (m_LosCircular) diff --git a/source/simulation2/components/CCmpTerritoryManager.cpp b/source/simulation2/components/CCmpTerritoryManager.cpp index f204afc5be..1a03f6bad6 100644 --- a/source/simulation2/components/CCmpTerritoryManager.cpp +++ b/source/simulation2/components/CCmpTerritoryManager.cpp @@ -35,7 +35,6 @@ #include "simulation2/components/ICmpPlayer.h" #include "simulation2/components/ICmpPlayerManager.h" #include "simulation2/components/ICmpPosition.h" -#include "simulation2/components/ICmpTerrain.h" #include "simulation2/components/ICmpTerritoryInfluence.h" #include "simulation2/helpers/Geometry.h" #include "simulation2/helpers/Grid.h" @@ -45,15 +44,14 @@ class CCmpTerritoryManager; -class TerritoryOverlay : public TerrainOverlay +class TerritoryOverlay : public TerrainTextureOverlay { NONCOPYABLE(TerritoryOverlay); public: CCmpTerritoryManager& m_TerritoryManager; TerritoryOverlay(CCmpTerritoryManager& manager); - virtual void StartRender(); - virtual void ProcessTile(ssize_t i, ssize_t j); + virtual void BuildTextureRGBA(u8* data, size_t w, size_t h); }; class CCmpTerritoryManager : public ICmpTerritoryManager @@ -88,6 +86,9 @@ public: // processed flag in bit 7 (TERRITORY_PROCESSED_MASK) Grid* m_Territories; + // Saves the cost per tile (to stop territory on impassable tiles) + Grid* m_CostGrid; + // Set to true when territories change; will send a TerritoriesChanged message // during the Update phase bool m_TriggerEvent; @@ -112,6 +113,7 @@ public: virtual void Init(const CParamNode& UNUSED(paramNode)) { m_Territories = NULL; + m_CostGrid = NULL; m_DebugOverlay = NULL; // m_DebugOverlay = new TerritoryOverlay(*this); m_BoundaryLinesDirty = true; @@ -137,6 +139,7 @@ public: virtual void Deinit() { SAFE_DELETE(m_Territories); + SAFE_DELETE(m_CostGrid); SAFE_DELETE(m_DebugOverlay); } @@ -177,6 +180,8 @@ public: case MT_TerrainChanged: case MT_WaterChanged: { + // also recalculate the cost grid to support atlas changes + SAFE_DELETE(m_CostGrid); MakeDirty(); break; } @@ -251,6 +256,8 @@ public: return false; } + void CalculateCostGrid(); + void CalculateTerritories(); std::vector ComputeBoundaries(); @@ -305,10 +312,50 @@ struct Tile /** * Compute the tile indexes on the grid nearest to a given point */ -static void NearestTile(entity_pos_t x, entity_pos_t z, u16& i, u16& j, u16 w, u16 h) +static void NearestTerritoryTile(entity_pos_t x, entity_pos_t z, u16& i, u16& j, u16 w, u16 h) { - i = (u16)clamp((x / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, w-1); - j = (u16)clamp((z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), 0, h-1); + entity_pos_t scale = Pathfinding::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE; + i = clamp((x / scale).ToInt_RoundToNegInfinity(), 0, w - 1); + j = clamp((z / scale).ToInt_RoundToNegInfinity(), 0, h - 1); +} + +void CCmpTerritoryManager::CalculateCostGrid() +{ + if (m_CostGrid) + return; + + CmpPtr cmpPathfinder(GetSystemEntity()); + if (!cmpPathfinder) + return; + + pass_class_t passClassTerritory = cmpPathfinder->GetPassabilityClass("territory"); + pass_class_t passClassUnrestricted = cmpPathfinder->GetPassabilityClass("unrestricted"); + + const Grid& passGrid = cmpPathfinder->GetPassabilityGrid(); + + int tilesW = passGrid.m_W / NAVCELLS_PER_TERRITORY_TILE; + int tilesH = passGrid.m_H / NAVCELLS_PER_TERRITORY_TILE; + + m_CostGrid = new Grid(tilesW, tilesH); + + for (int i = 0; i < tilesW; ++i) + { + for (int j = 0; j < tilesH; ++j) + { + u16 c = 0; + for (u16 di = 0; di < NAVCELLS_PER_TERRITORY_TILE; ++di) + for (u16 dj = 0; dj < NAVCELLS_PER_TERRITORY_TILE; ++dj) + c |= passGrid.get( + i * NAVCELLS_PER_TERRITORY_TILE + di, + j * NAVCELLS_PER_TERRITORY_TILE + dj); + if (c & passClassTerritory) + m_CostGrid->set(i, j, m_ImpassableCost); + else if (c & passClassUnrestricted) + m_CostGrid->set(i, j, 255); // off the world; use maximum cost + else + m_CostGrid->set(i, j, 1); + } + } } void CCmpTerritoryManager::CalculateTerritories() @@ -318,15 +365,14 @@ void CCmpTerritoryManager::CalculateTerritories() PROFILE("CalculateTerritories"); - CmpPtr cmpTerrain(GetSystemEntity()); - - // If the terrain hasn't been loaded (e.g. this is called during map initialisation), + // If the pathfinder hasn't been loaded (e.g. this is called during map initialisation), // abort the computation (and assume callers can cope with m_Territories == NULL) - if (!cmpTerrain->IsLoaded()) + CalculateCostGrid(); + if (!m_CostGrid) return; - const u16 tilesW = cmpTerrain->GetTilesPerSide(); - const u16 tilesH = tilesW; + const u16 tilesW = m_CostGrid->m_W; + const u16 tilesH = m_CostGrid->m_H; m_Territories = new Grid(tilesW, tilesH); @@ -356,15 +402,6 @@ void CCmpTerritoryManager::CalculateTerritories() // store the root influences to mark territory as connected std::vector rootInfluenceEntities; - CmpPtr cmpPathfinder(GetSystemEntity()); - if (!cmpPathfinder) - return; - - ICmpPathfinder::pass_class_t passClassDefault = cmpPathfinder->GetPassabilityClass("default"); - ICmpPathfinder::pass_class_t passClassUnrestricted = cmpPathfinder->GetPassabilityClass("unrestricted"); - - const Grid& passibilityGrid = cmpPathfinder->GetPassabilityGrid(); - for (const std::pair >& pair : influenceEntities) { // entityGrid stores the weight for a single entity, and is reset per entity @@ -385,14 +422,14 @@ void CCmpTerritoryManager::CalculateTerritories() CmpPtr cmpTerritoryInfluence(GetSimContext(), ent); u32 weight = cmpTerritoryInfluence->GetWeight(); - u32 radius = cmpTerritoryInfluence->GetRadius() / TERRAIN_TILE_SIZE; + u32 radius = cmpTerritoryInfluence->GetRadius() / (Pathfinding::NAVCELL_SIZE * NAVCELLS_PER_TERRITORY_TILE).ToInt_RoundToNegInfinity(); if (weight == 0 || radius == 0) continue; u32 falloff = weight / radius; CFixedVector2D pos = cmpPosition->GetPosition2D(); u16 i, j; - NearestTile(pos.X, pos.Y, i, j, tilesW, tilesH); + NearestTerritoryTile(pos.X, pos.Y, i, j, tilesW, tilesH); if (cmpTerritoryInfluence->IsRoot()) rootInfluenceEntities.push_back(ent); @@ -407,13 +444,7 @@ void CCmpTerritoryManager::CalculateTerritories() // Expand influences outwards FLOODFILL(i, j, - u32 dg = falloff; - // enlarge the falloff for unpassable tiles - u16 g = passibilityGrid.get(nx, nz); - if (g & passClassUnrestricted) - dg *= 255; // off the world; use maximum cost - else if (g & passClassDefault) - dg *= m_ImpassableCost; + u32 dg = falloff * m_CostGrid->get(nx, nz); // diagonal neighbour -> multiply with approx sqrt(2) if (nx != x && nz != z) @@ -451,7 +482,7 @@ void CCmpTerritoryManager::CalculateTerritories() CFixedVector2D pos = cmpPosition->GetPosition2D(); u16 i, j; - NearestTile(pos.X, pos.Y, i, j, tilesW, tilesH); + NearestTerritoryTile(pos.X, pos.Y, i, j, tilesW, tilesH); u8 owner = (u8)cmpOwnership->GetOwner(); @@ -592,7 +623,7 @@ player_id_t CCmpTerritoryManager::GetOwner(entity_pos_t x, entity_pos_t z) if (!m_Territories) return 0; - NearestTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H); + NearestTerritoryTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H); return m_Territories->get(i, j) & TERRITORY_PLAYER_MASK; } @@ -608,7 +639,7 @@ std::vector CCmpTerritoryManager::GetNeighbours(entity_pos_t x, entity_pos_ return ret; u16 i, j; - NearestTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H); + NearestTerritoryTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H); // calculate the neighbours player_id_t thisOwner = m_Territories->get(i, j) & TERRITORY_PLAYER_MASK; @@ -644,7 +675,7 @@ bool CCmpTerritoryManager::IsConnected(entity_pos_t x, entity_pos_t z) if (!m_Territories) return false; - NearestTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H); + NearestTerritoryTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H); return (m_Territories->get(i, j) & TERRITORY_CONNECTED_MASK) != 0; } @@ -655,7 +686,7 @@ void CCmpTerritoryManager::SetTerritoryBlinking(entity_pos_t x, entity_pos_t z) return; u16 i, j; - NearestTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H); + NearestTerritoryTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H); u16 tilesW = m_Territories->m_W; u16 tilesH = m_Territories->m_H; @@ -671,33 +702,25 @@ void CCmpTerritoryManager::SetTerritoryBlinking(entity_pos_t x, entity_pos_t z) m_BoundaryLinesDirty = true; } -TerritoryOverlay::TerritoryOverlay(CCmpTerritoryManager& manager) - : TerrainOverlay(manager.GetSimContext()), m_TerritoryManager(manager) +TerritoryOverlay::TerritoryOverlay(CCmpTerritoryManager& manager) : + TerrainTextureOverlay((float)Pathfinding::NAVCELLS_PER_TILE / ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE), + m_TerritoryManager(manager) { } -void TerritoryOverlay::StartRender() +void TerritoryOverlay::BuildTextureRGBA(u8* data, size_t w, size_t h) { - m_TerritoryManager.CalculateTerritories(); -} - -void TerritoryOverlay::ProcessTile(ssize_t i, ssize_t j) -{ - if (!m_TerritoryManager.m_Territories) - return; - - u8 id = (m_TerritoryManager.m_Territories->get((int) i, (int) j) & ICmpTerritoryManager::TERRITORY_PLAYER_MASK); - - float a = 0.2f; - switch (id) + for (size_t j = 0; j < h; ++j) { - case 0: break; - case 1: RenderTile(CColor(1, 0, 0, a), false); break; - case 2: RenderTile(CColor(0, 1, 0, a), false); break; - case 3: RenderTile(CColor(0, 0, 1, a), false); break; - case 4: RenderTile(CColor(1, 1, 0, a), false); break; - case 5: RenderTile(CColor(0, 1, 1, a), false); break; - case 6: RenderTile(CColor(1, 0, 1, a), false); break; - default: RenderTile(CColor(1, 1, 1, a), false); break; + for (size_t i = 0; i < w; ++i) + { + SColor4ub color; + u8 id = (m_TerritoryManager.m_Territories->get((int)i, (int)j) & ICmpTerritoryManager::TERRITORY_PLAYER_MASK); + color = GetColor(id, 64); + *data++ = color.R; + *data++ = color.G; + *data++ = color.B; + *data++ = color.A; + } } } diff --git a/source/simulation2/components/CCmpUnitMotion.cpp b/source/simulation2/components/CCmpUnitMotion.cpp index ad7f093c8c..d3ed264f8a 100644 --- a/source/simulation2/components/CCmpUnitMotion.cpp +++ b/source/simulation2/components/CCmpUnitMotion.cpp @@ -39,6 +39,10 @@ #include "ps/Profile.h" #include "renderer/Scene.h" +// For debugging; units will start going straight to the target +// instead of calling the pathfinder +#define DISABLE_PATHFINDER 0 + /** * When advancing along the long path, and picking a new waypoint to move * towards, we'll pick one that's up to this far from the unit's current @@ -59,7 +63,7 @@ static const int WAYPOINT_ADVANCE_LOOKAHEAD_TURNS = 4; * Maximum range to restrict short path queries to. (Larger ranges are slower, * smaller ranges might miss some legitimate routes around large obstacles.) */ -static const entity_pos_t SHORT_PATH_SEARCH_RANGE = entity_pos_t::FromInt(TERRAIN_TILE_SIZE*10); +static const entity_pos_t SHORT_PATH_SEARCH_RANGE = entity_pos_t::FromInt(TERRAIN_TILE_SIZE*6); /** * When short-pathing to an intermediate waypoint, we aim for a circle of this radius @@ -107,7 +111,30 @@ static const fixed CHECK_TARGET_MOVEMENT_MIN_COS = fixed::FromInt(866)/1000; static const CColor OVERLAY_COLOR_LONG_PATH(1, 1, 1, 1); static const CColor OVERLAY_COLOR_SHORT_PATH(1, 0, 0, 1); -static const entity_pos_t g_GoalDelta = entity_pos_t::FromInt(TERRAIN_TILE_SIZE)/4; // for extending the goal outwards/inwards a little bit +static const entity_pos_t g_GoalDelta = Pathfinding::NAVCELL_SIZE; // for extending the goal outwards/inwards a little bit + +struct SUnitMotionPlanning +{ + WaypointPath nextStepShortPath; // if !nextStepClean, store a short path for the next step here + u32 expectedPathTicket; + bool nextStepClean; // is there any obstruction between the next two long waypoints? + + SUnitMotionPlanning() : expectedPathTicket(0), nextStepClean(true) {} +}; + +/** + * Serialization helper template for SUnitMotionPlanning + */ +struct SerializeUnitMotionPlanning +{ + template + void operator()(S& serialize, const char* UNUSED(name), SUnitMotionPlanning& value) + { + SerializeVector()(serialize, "next step short path", value.nextStepShortPath.m_Waypoints); + serialize.NumberU32_Unbounded("expected path ticket", value.expectedPathTicket); + serialize.Bool("next step clean", value.nextStepClean); + } +}; class CCmpUnitMotion : public ICmpUnitMotion { @@ -132,9 +159,8 @@ public: bool m_FormationController; fixed m_WalkSpeed, m_OriginalWalkSpeed; // in metres per second fixed m_RunSpeed, m_OriginalRunSpeed; - ICmpPathfinder::pass_class_t m_PassClass; + pass_class_t m_PassClass; std::string m_PassClassName; - ICmpPathfinder::cost_class_t m_CostClass; // Dynamic state: @@ -217,15 +243,6 @@ public: */ PATHSTATE_FOLLOWING_REQUESTING_SHORT, - /* - * We are following our path, and have an outstanding short path request - * to append to our current path. - * (This is because we got near the end of our short path and need - * to extend it to continue along the long path). - * m_LongPath and m_ShortPath are valid. - */ - PATHSTATE_FOLLOWING_REQUESTING_SHORT_APPEND, - PATHSTATE_MAX }; u8 m_PathState; @@ -245,10 +262,13 @@ public: // Currently active paths (storing waypoints in reverse order). // The last item in each path is the point we're currently heading towards. - ICmpPathfinder::Path m_LongPath; - ICmpPathfinder::Path m_ShortPath; + WaypointPath m_LongPath; + WaypointPath m_ShortPath; - ICmpPathfinder::Goal m_FinalGoal; + // Motion planning + SUnitMotionPlanning m_Planning; + + PathGoal m_FinalGoal; static std::string GetSchema() { @@ -257,7 +277,6 @@ public: "" "7.0" "default" - "infantry" "" "" "" @@ -278,9 +297,6 @@ public: "" "" "" - "" - "" - "" ""; } @@ -304,18 +320,19 @@ public: else m_RunSpeed = m_OriginalRunSpeed = m_WalkSpeed; + CmpPtr cmpObstruction(GetEntityHandle()); + if (cmpObstruction) + m_Radius = cmpObstruction->GetUnitRadius(); + CmpPtr cmpPathfinder(GetSystemEntity()); if (cmpPathfinder) { m_PassClassName = paramNode.GetChild("PassabilityClass").ToUTF8(); m_PassClass = cmpPathfinder->GetPassabilityClass(m_PassClassName); - m_CostClass = cmpPathfinder->GetCostClass(paramNode.GetChild("CostClass").ToUTF8()); + if (cmpObstruction) + cmpObstruction->SetUnitClearance(cmpPathfinder->GetClearance(m_PassClass)); } - CmpPtr cmpObstruction(GetEntityHandle()); - if (cmpObstruction) - m_Radius = cmpObstruction->GetUnitRadius(); - m_State = STATE_IDLE; m_PathState = PATHSTATE_NONE; @@ -323,7 +340,7 @@ public: m_TargetEntity = INVALID_ENTITY; - m_FinalGoal.type = ICmpPathfinder::Goal::POINT; + m_FinalGoal.type = PathGoal::POINT; m_DebugOverlayEnabled = false; } @@ -360,6 +377,8 @@ public: SerializeVector()(serialize, "long path", m_LongPath.m_Waypoints); SerializeVector()(serialize, "short path", m_ShortPath.m_Waypoints); + SerializeUnitMotionPlanning()(serialize, "planning", m_Planning); + SerializeGoal()(serialize, "goal", m_FinalGoal); } @@ -464,7 +483,7 @@ public: return m_RunSpeed; } - virtual ICmpPathfinder::pass_class_t GetPassabilityClass() + virtual pass_class_t GetPassabilityClass() { return m_PassClass; } @@ -527,12 +546,12 @@ public: } private: - bool ShouldAvoidMovingUnits() + bool ShouldAvoidMovingUnits() const { return !m_FormationController; } - bool IsFormationMember() + bool IsFormationMember() const { return m_State == STATE_FORMATIONMEMBER_PATH; } @@ -588,18 +607,24 @@ private: /** * Handle the result of an asynchronous path query. */ - void PathResult(u32 ticket, const ICmpPathfinder::Path& path); + void PathResult(u32 ticket, const WaypointPath& path); /** * Do the per-turn movement and other updates. */ void Move(fixed dt); + /** + * Analyse the next long path step (if any) and precompute a short path if needed. + * Then use the previous computed short path, if present, for the current step. + */ + void PlanNextStep(const CFixedVector2D& pos); + /** * Decide whether to approximate the given range from a square target as a circle, * rather than as a square. */ - bool ShouldTreatTargetAsCircle(entity_pos_t range, entity_pos_t hw, entity_pos_t hh, entity_pos_t circleRadius); + bool ShouldTreatTargetAsCircle(entity_pos_t range, entity_pos_t hw, entity_pos_t hh, entity_pos_t circleRadius) const; /** * Computes the current location of our target entity (plus offset). @@ -607,70 +632,79 @@ private: */ bool ComputeTargetPosition(CFixedVector2D& out); + /** + * Attempts to replace the current path with a straight line to the goal, + * if this goal is a point, is close enough and the route is not obstructed. + */ + bool TryGoingStraightToGoalPoint(const CFixedVector2D& from); + /** * Attempts to replace the current path with a straight line to the target * entity, if it's close enough and the route is not obstructed. */ - bool TryGoingStraightToTargetEntity(CFixedVector2D from); + bool TryGoingStraightToTargetEntity(const CFixedVector2D& from); /** * Returns whether the target entity has moved more than minDelta since our * last path computations, and we're close enough to it to care. */ - bool CheckTargetMovement(CFixedVector2D from, entity_pos_t minDelta); + bool CheckTargetMovement(const CFixedVector2D& from, entity_pos_t minDelta); /** * Returns whether the length of the given path, plus the distance from * 'from' to the first waypoints, it shorter than minDistance. */ - bool PathIsShort(const ICmpPathfinder::Path& path, CFixedVector2D from, entity_pos_t minDistance); + bool PathIsShort(const WaypointPath& path, const CFixedVector2D& from, entity_pos_t minDistance) const; /** * Rotate to face towards the target point, given the current pos */ - void FaceTowardsPointFromPos(CFixedVector2D pos, entity_pos_t x, entity_pos_t z); + void FaceTowardsPointFromPos(const CFixedVector2D& pos, entity_pos_t x, entity_pos_t z); /** * Returns an appropriate obstruction filter for use with path requests. */ - ControlGroupMovementObstructionFilter GetObstructionFilter(bool forceAvoidMovingUnits = false); + ControlGroupMovementObstructionFilter GetObstructionFilter(bool forceAvoidMovingUnits = false) const; /** * Start moving to the given goal, from our current position 'from'. * Might go in a straight line immediately, or might start an asynchronous * path request. */ - void BeginPathing(CFixedVector2D from, const ICmpPathfinder::Goal& goal); + void BeginPathing(const CFixedVector2D& from, const PathGoal& goal); /** * Start an asynchronous long path query. */ - void RequestLongPath(CFixedVector2D from, const ICmpPathfinder::Goal& goal); + void RequestLongPath(const CFixedVector2D& from, const PathGoal& goal); /** * Start an asynchronous short path query. */ - void RequestShortPath(CFixedVector2D from, const ICmpPathfinder::Goal& goal, bool avoidMovingUnits); - - /** - * Select a next long waypoint, given the current unit position. - * Also recomputes the short path to use that waypoint. - * Returns false on error, or if there is no waypoint to pick. - */ - bool PickNextLongWaypoint(const CFixedVector2D& pos, bool avoidMovingUnits); + void RequestShortPath(const CFixedVector2D& from, const PathGoal& goal, bool avoidMovingUnits); /** * Convert a path into a renderable list of lines */ - void RenderPath(const ICmpPathfinder::Path& path, std::vector& lines, CColor color); + void RenderPath(const WaypointPath& path, std::vector& lines, CColor color); void RenderSubmit(SceneCollector& collector); }; REGISTER_COMPONENT_TYPE(UnitMotion) -void CCmpUnitMotion::PathResult(u32 ticket, const ICmpPathfinder::Path& path) +void CCmpUnitMotion::PathResult(u32 ticket, const WaypointPath& path) { + if (ticket == m_Planning.expectedPathTicket) + { + // If no path was found, better cancel the planning + if (path.m_Waypoints.empty()) + m_Planning = SUnitMotionPlanning(); + + m_Planning.nextStepShortPath = path; + return; + } + // Ignore obsolete path requests if (ticket != m_ExpectedPathTicket) return; @@ -687,10 +721,7 @@ void CCmpUnitMotion::PathResult(u32 ticket, const ICmpPathfinder::Path& path) // (via the short pathfinder), so if we're stuck and the user clicks // close enough to the unit then we can probably get unstuck if (m_LongPath.m_Waypoints.empty()) - { - ICmpPathfinder::Waypoint wp = { m_FinalGoal.x, m_FinalGoal.z }; - m_LongPath.m_Waypoints.push_back(wp); - } + m_LongPath.m_Waypoints.emplace_back(Waypoint{ m_FinalGoal.x, m_FinalGoal.z }); CmpPtr cmpPosition(GetEntityHandle()); if (!cmpPosition || !cmpPosition->IsInWorld()) @@ -699,16 +730,8 @@ void CCmpUnitMotion::PathResult(u32 ticket, const ICmpPathfinder::Path& path) return; } - CFixedVector2D pos = cmpPosition->GetPosition2D(); - - if (!PickNextLongWaypoint(pos, ShouldAvoidMovingUnits())) - { - StartFailed(); - return; - } - - // We started a short path request to the next long path waypoint - m_PathState = PATHSTATE_WAITING_REQUESTING_SHORT; + m_PathState = PATHSTATE_FOLLOWING; + StartSucceeded(); } else if (m_PathState == PATHSTATE_WAITING_REQUESTING_SHORT) { @@ -717,7 +740,10 @@ void CCmpUnitMotion::PathResult(u32 ticket, const ICmpPathfinder::Path& path) // If there's no waypoints then we couldn't get near the target if (m_ShortPath.m_Waypoints.empty()) { - if (!IsFormationMember()) + // If we're globally following a long path, try to remove the next waypoint, it might be obstructed + if (m_LongPath.m_Waypoints.size() > 1) + m_LongPath.m_Waypoints.pop_back(); + else if (!IsFormationMember()) { StartFailed(); return; @@ -753,10 +779,7 @@ void CCmpUnitMotion::PathResult(u32 ticket, const ICmpPathfinder::Path& path) // (via the short pathfinder), so if we're stuck and the user clicks // close enough to the unit then we can probably get unstuck if (m_LongPath.m_Waypoints.empty()) - { - ICmpPathfinder::Waypoint wp = { m_FinalGoal.x, m_FinalGoal.z }; - m_LongPath.m_Waypoints.push_back(wp); - } + m_LongPath.m_Waypoints.emplace_back(Waypoint{ m_FinalGoal.x, m_FinalGoal.z }); CmpPtr cmpPosition(GetEntityHandle()); if (!cmpPosition || !cmpPosition->IsInWorld()) @@ -765,16 +788,7 @@ void CCmpUnitMotion::PathResult(u32 ticket, const ICmpPathfinder::Path& path) return; } - CFixedVector2D pos = cmpPosition->GetPosition2D(); - - if (!PickNextLongWaypoint(pos, ShouldAvoidMovingUnits())) - { - StopMoving(); - return; - } - - // We started a short path request to the next long path waypoint - m_PathState = PATHSTATE_FOLLOWING_REQUESTING_SHORT; + m_PathState = PATHSTATE_FOLLOWING; // (TODO: is this entirely safe? We might continue moving along our // old path while this request is active, so it'll be slightly incorrect @@ -805,20 +819,6 @@ void CCmpUnitMotion::PathResult(u32 ticket, const ICmpPathfinder::Path& path) m_PathState = PATHSTATE_FOLLOWING; } - else if (m_PathState == PATHSTATE_FOLLOWING_REQUESTING_SHORT_APPEND) - { - // Append the new path onto our current one - m_ShortPath.m_Waypoints.insert(m_ShortPath.m_Waypoints.begin(), path.m_Waypoints.begin(), path.m_Waypoints.end()); - - // If there's no waypoints then we couldn't get near the target - // from the last intermediate long-path waypoint. But we can still - // continue using the remainder of our current short path. So just - // discard the now-useless long path. - if (path.m_Waypoints.empty()) - m_LongPath.m_Waypoints.clear(); - - m_PathState = PATHSTATE_FOLLOWING; - } else { LOGWARNING("unexpected PathResult (%u %d %d)", GetEntityId(), m_State, m_PathState); @@ -856,7 +856,6 @@ void CCmpUnitMotion::Move(fixed dt) case PATHSTATE_FOLLOWING: case PATHSTATE_FOLLOWING_REQUESTING_SHORT: - case PATHSTATE_FOLLOWING_REQUESTING_SHORT_APPEND: case PATHSTATE_FOLLOWING_REQUESTING_LONG: { // TODO: there's some asymmetry here when units look at other @@ -894,7 +893,8 @@ void CCmpUnitMotion::Move(fixed dt) // Find the speed factor of the underlying terrain // (We only care about the tile we start on - it doesn't matter if we're moving // partially onto a much slower/faster tile) - fixed terrainSpeed = cmpPathfinder->GetMovementSpeed(pos.X, pos.Y, m_CostClass); + // TODO: Terrain-dependent speeds are not currently supported + fixed terrainSpeed = fixed::FromInt(1); fixed maxSpeed = basicSpeed.Multiply(terrainSpeed); @@ -907,11 +907,16 @@ void CCmpUnitMotion::Move(fixed dt) while (timeLeft > zero) { - // If we ran out of short path, we have to stop - if (m_ShortPath.m_Waypoints.empty()) + // If we ran out of path, we have to stop + if (m_ShortPath.m_Waypoints.empty() && m_LongPath.m_Waypoints.empty()) break; - CFixedVector2D target(m_ShortPath.m_Waypoints.back().x, m_ShortPath.m_Waypoints.back().z); + CFixedVector2D target; + if (m_ShortPath.m_Waypoints.empty()) + target = CFixedVector2D(m_LongPath.m_Waypoints.back().x, m_LongPath.m_Waypoints.back().z); + else + target = CFixedVector2D(m_ShortPath.m_Waypoints.back().x, m_ShortPath.m_Waypoints.back().z); + CFixedVector2D offset = target - pos; // Work out how far we can travel in timeLeft @@ -928,7 +933,14 @@ void CCmpUnitMotion::Move(fixed dt) // Spend the rest of the time heading towards the next waypoint timeLeft = timeLeft - (offsetLength / maxSpeed); - m_ShortPath.m_Waypoints.pop_back(); + if (m_ShortPath.m_Waypoints.empty()) + { + m_LongPath.m_Waypoints.pop_back(); + PlanNextStep(pos); + } + else + m_ShortPath.m_Waypoints.pop_back(); + continue; } else @@ -977,11 +989,21 @@ void CCmpUnitMotion::Move(fixed dt) // Stop, and recompute the whole path. // TODO: if the target has UnitMotion and is higher priority, // we should wait a little bit. - - m_CurSpeed = zero; - RequestLongPath(pos, m_FinalGoal); - m_PathState = PATHSTATE_WAITING_REQUESTING_LONG; + // Recompute our path + // If we are following a long path + if (!m_LongPath.m_Waypoints.empty()) + { + m_ShortPath.m_Waypoints.clear(); + PathGoal goal = { PathGoal::POINT, m_LongPath.m_Waypoints.back().x, m_LongPath.m_Waypoints.back().z }; + RequestShortPath(pos, goal, true); + m_PathState = PATHSTATE_WAITING_REQUESTING_SHORT; + return; + } + // Else, just entirely recompute + BeginPathing(pos, m_FinalGoal); + + // TODO: check where the collision was and move slightly. return; } @@ -992,64 +1014,38 @@ void CCmpUnitMotion::Move(fixed dt) { // If we're not currently computing any new paths: - // If we are close to reaching the end of the short path - // (or have reached it already), try to extend it - - entity_pos_t minDistance = basicSpeed.Multiply(dt) * WAYPOINT_ADVANCE_LOOKAHEAD_TURNS; - if (PathIsShort(m_ShortPath, pos, minDistance)) + if (m_LongPath.m_Waypoints.empty() && m_ShortPath.m_Waypoints.empty()) { - // Start the path extension from the end of this short path - // (or our current position if no short path) - CFixedVector2D from = pos; - if (!m_ShortPath.m_Waypoints.empty()) - from = CFixedVector2D(m_ShortPath.m_Waypoints[0].x, m_ShortPath.m_Waypoints[0].z); - - if (PickNextLongWaypoint(from, ShouldAvoidMovingUnits())) + if (IsFormationMember()) { - m_PathState = PATHSTATE_FOLLOWING_REQUESTING_SHORT_APPEND; + // We've reached our assigned position. If the controller + // is idle, send a notification in case it should disband, + // otherwise continue following the formation next turn. + CmpPtr cmpUnitMotion(GetSimContext(), m_TargetEntity); + if (cmpUnitMotion && !cmpUnitMotion->IsMoving()) + { + m_Moving = false; + CMessageMotionChanged msg(false, false); + GetSimContext().GetComponentManager().PostMessage(GetEntityId(), msg); + } } else { - // Failed (there were no long waypoints left). - // If there's still some short path then continue following - // it, else we've finished moving. - if (m_ShortPath.m_Waypoints.empty()) - { - if (IsFormationMember()) - { - // We've reached our assigned position. If the controller - // is idle, send a notification in case it should disband, - // otherwise continue following the formation next turn. - CmpPtr cmpUnitMotion(GetSimContext(), m_TargetEntity); - if (cmpUnitMotion && !cmpUnitMotion->IsMoving()) - { - m_Moving = false; - CMessageMotionChanged msg(false, false); - GetSimContext().GetComponentManager().PostMessage(GetEntityId(), msg); - } - } - else - { - // check if target was reached in case of a moving target - CmpPtr cmpUnitMotion(GetSimContext(), m_TargetEntity); - if - ( - cmpUnitMotion && cmpUnitMotion->IsMoving() && - MoveToTargetRange(m_TargetEntity, m_TargetMinRange, m_TargetMaxRange) - ) - return; + // check if target was reached in case of a moving target + CmpPtr cmpUnitMotion(GetSimContext(), m_TargetEntity); + if (cmpUnitMotion && cmpUnitMotion->IsMoving() && + MoveToTargetRange(m_TargetEntity, m_TargetMinRange, m_TargetMaxRange)) + return; - // Not in formation, so just finish moving - StopMoving(); - m_State = STATE_IDLE; - MoveSucceeded(); + // Not in formation, so just finish moving + StopMoving(); + m_State = STATE_IDLE; + MoveSucceeded(); - if (m_FacePointAfterMove) - FaceTowardsPointFromPos(pos, m_FinalGoal.x, m_FinalGoal.z); - // TODO: if the goal was a square building, we ought to point towards the - // nearest point on the square, not towards its center - } - } + if (m_FacePointAfterMove) + FaceTowardsPointFromPos(pos, m_FinalGoal.x, m_FinalGoal.z); + // TODO: if the goal was a square building, we ought to point towards the + // nearest point on the square, not towards its center } } } @@ -1068,6 +1064,44 @@ void CCmpUnitMotion::Move(fixed dt) } } +void CCmpUnitMotion::PlanNextStep(const CFixedVector2D& pos) +{ + if (m_LongPath.m_Waypoints.empty()) + return; + + CmpPtr cmpPathfinder(GetSystemEntity()); + + const Waypoint& nextPoint = m_LongPath.m_Waypoints.back(); + + // The next step was obstructed the last time we checked; also check that + // the step is still obstructed (maybe the units in our way moved in the meantime) + if (!m_Planning.nextStepClean && + !cmpPathfinder->CheckMovement(GetObstructionFilter(), pos.X, pos.Y, nextPoint.x, nextPoint.z, m_Radius, m_PassClass)) + { + // If the short path computation is over, use it, else just forget about it + if (!m_Planning.nextStepShortPath.m_Waypoints.empty()) + { + m_PathState = PATHSTATE_FOLLOWING; + m_ShortPath = m_Planning.nextStepShortPath; + } + } + + m_Planning = SUnitMotionPlanning(); + + if (m_LongPath.m_Waypoints.size() == 1) + return; + + const Waypoint& followingPoint = m_LongPath.m_Waypoints.rbegin()[1]; // penultimate element + m_Planning.nextStepClean = cmpPathfinder->CheckMovement( + GetObstructionFilter(), nextPoint.x, nextPoint.z, followingPoint.x, followingPoint.z, m_Radius, m_PassClass); + if (!m_Planning.nextStepClean) + { + PathGoal goal = { PathGoal::POINT, followingPoint.x, followingPoint.z }; + m_Planning.expectedPathTicket = cmpPathfinder->ComputeShortPathAsync( + nextPoint.x, nextPoint.z, m_Radius, SHORT_PATH_SEARCH_RANGE, goal, m_PassClass, false, m_TargetEntity, GetEntityId()); + } +} + bool CCmpUnitMotion::ComputeTargetPosition(CFixedVector2D& out) { if (m_TargetEntity == INVALID_ENTITY) @@ -1092,7 +1126,34 @@ bool CCmpUnitMotion::ComputeTargetPosition(CFixedVector2D& out) return true; } -bool CCmpUnitMotion::TryGoingStraightToTargetEntity(CFixedVector2D from) +bool CCmpUnitMotion::TryGoingStraightToGoalPoint(const CFixedVector2D& from) +{ + // Make sure the goal is a point + if (m_FinalGoal.type != PathGoal::POINT) + return false; + + // Fail if the goal is too far away + CFixedVector2D goalPos(m_FinalGoal.x, m_FinalGoal.z); + if ((goalPos - from).CompareLength(DIRECT_PATH_RANGE) > 0) + return false; + + CmpPtr cmpPathfinder(GetSystemEntity()); + if (!cmpPathfinder) + return false; + + // Check if there's any collisions on that route + if (!cmpPathfinder->CheckMovement(GetObstructionFilter(), from.X, from.Y, goalPos.X, goalPos.Y, m_Radius, m_PassClass)) + return false; + + // That route is okay, so update our path + m_LongPath.m_Waypoints.clear(); + m_ShortPath.m_Waypoints.clear(); + m_ShortPath.m_Waypoints.emplace_back(Waypoint{ goalPos.X, goalPos.Y }); + + return true; +} + +bool CCmpUnitMotion::TryGoingStraightToTargetEntity(const CFixedVector2D& from) { CFixedVector2D targetPos; if (!ComputeTargetPosition(targetPos)) @@ -1107,14 +1168,14 @@ bool CCmpUnitMotion::TryGoingStraightToTargetEntity(CFixedVector2D from) return false; // Move the goal to match the target entity's new position - ICmpPathfinder::Goal goal = m_FinalGoal; + PathGoal goal = m_FinalGoal; goal.x = targetPos.X; goal.z = targetPos.Y; // (we ignore changes to the target's rotation, since only buildings are // square and buildings don't move) // Find the point on the goal shape that we should head towards - CFixedVector2D goalPos = cmpPathfinder->GetNearestPointOnGoal(from, goal); + CFixedVector2D goalPos = goal.NearestPointOnGoal(from); // Check if there's any collisions on that route if (!cmpPathfinder->CheckMovement(GetObstructionFilter(), from.X, from.Y, goalPos.X, goalPos.Y, m_Radius, m_PassClass)) @@ -1124,13 +1185,12 @@ bool CCmpUnitMotion::TryGoingStraightToTargetEntity(CFixedVector2D from) m_FinalGoal = goal; m_LongPath.m_Waypoints.clear(); m_ShortPath.m_Waypoints.clear(); - ICmpPathfinder::Waypoint wp = { goalPos.X, goalPos.Y }; - m_ShortPath.m_Waypoints.push_back(wp); + m_ShortPath.m_Waypoints.emplace_back(Waypoint{ goalPos.X, goalPos.Y }); return true; } -bool CCmpUnitMotion::CheckTargetMovement(CFixedVector2D from, entity_pos_t minDelta) +bool CCmpUnitMotion::CheckTargetMovement(const CFixedVector2D& from, entity_pos_t minDelta) { CFixedVector2D targetPos; if (!ComputeTargetPosition(targetPos)) @@ -1179,21 +1239,22 @@ bool CCmpUnitMotion::CheckTargetMovement(CFixedVector2D from, entity_pos_t minDe return true; } -bool CCmpUnitMotion::PathIsShort(const ICmpPathfinder::Path& path, CFixedVector2D from, entity_pos_t minDistance) +bool CCmpUnitMotion::PathIsShort(const WaypointPath& path, const CFixedVector2D& from, entity_pos_t minDistance) const { + CFixedVector2D prev = from; entity_pos_t distLeft = minDistance; for (ssize_t i = (ssize_t)path.m_Waypoints.size()-1; i >= 0; --i) { // Check if the next path segment is longer than the requested minimum CFixedVector2D waypoint(path.m_Waypoints[i].x, path.m_Waypoints[i].z); - CFixedVector2D delta = waypoint - from; + CFixedVector2D delta = waypoint - prev; if (delta.CompareLength(distLeft) > 0) return false; // Still short enough - prepare to check the next segment distLeft -= delta.Length(); - from = waypoint; + prev = waypoint; } // Reached the end of the path before exceeding minDistance @@ -1210,7 +1271,7 @@ void CCmpUnitMotion::FaceTowardsPoint(entity_pos_t x, entity_pos_t z) FaceTowardsPointFromPos(pos, x, z); } -void CCmpUnitMotion::FaceTowardsPointFromPos(CFixedVector2D pos, entity_pos_t x, entity_pos_t z) +void CCmpUnitMotion::FaceTowardsPointFromPos(const CFixedVector2D& pos, entity_pos_t x, entity_pos_t z) { CFixedVector2D target(x, z); CFixedVector2D offset = target - pos; @@ -1225,7 +1286,7 @@ void CCmpUnitMotion::FaceTowardsPointFromPos(CFixedVector2D pos, entity_pos_t x, } } -ControlGroupMovementObstructionFilter CCmpUnitMotion::GetObstructionFilter(bool forceAvoidMovingUnits) +ControlGroupMovementObstructionFilter CCmpUnitMotion::GetObstructionFilter(bool forceAvoidMovingUnits) const { entity_id_t group; if (IsFormationMember()) @@ -1238,7 +1299,7 @@ ControlGroupMovementObstructionFilter CCmpUnitMotion::GetObstructionFilter(bool -void CCmpUnitMotion::BeginPathing(CFixedVector2D from, const ICmpPathfinder::Goal& goal) +void CCmpUnitMotion::BeginPathing(const CFixedVector2D& from, const PathGoal& goal) { // Cancel any pending path requests m_ExpectedPathTicket = 0; @@ -1251,6 +1312,18 @@ void CCmpUnitMotion::BeginPathing(CFixedVector2D from, const ICmpPathfinder::Goa if (cmpObstruction) cmpObstruction->SetMovingFlag(true); +#if DISABLE_PATHFINDER + { + CmpPtr cmpPathfinder (GetSimContext(), SYSTEM_ENTITY); + CFixedVector2D goalPos = cmpPathfinder->GetNearestPointOnGoal(from, m_FinalGoal); + m_LongPath.m_Waypoints.clear(); + m_ShortPath.m_Waypoints.clear(); + m_ShortPath.m_Waypoints.emplace_back(Waypoint{ goalPos.X, goalPos.Y }); + m_PathState = PATHSTATE_FOLLOWING; + return; + } +#endif + // If we're aiming at a target entity and it's close and we can reach // it in a straight line, then we'll just go along the straight line // instead of computing a path. @@ -1260,30 +1333,46 @@ void CCmpUnitMotion::BeginPathing(CFixedVector2D from, const ICmpPathfinder::Goa return; } - // TODO: should go straight to non-entity points too + // Same thing applies to non-entity points + if (TryGoingStraightToGoalPoint(from)) + { + m_PathState = PATHSTATE_FOLLOWING; + return; + } // Otherwise we need to compute a path. - // TODO: if it's close then just do a short path, not a long path - // (But if it's close on the opposite side of a river then we really - // need a long path, so we can't simply check linear distance) - - m_PathState = PATHSTATE_WAITING_REQUESTING_LONG; - RequestLongPath(from, goal); + // If it's close then just do a short path, not a long path + // TODO: If it's close on the opposite side of a river then we really + // need a long path, so we shouldn't simply check linear distance + if (goal.DistanceToPoint(from) < SHORT_PATH_SEARCH_RANGE) + { + m_LongPath.m_Waypoints.clear(); + m_PathState = PATHSTATE_WAITING_REQUESTING_SHORT; + RequestShortPath(from, goal, true); + } + else + { + m_PathState = PATHSTATE_WAITING_REQUESTING_LONG; + RequestLongPath(from, goal); + } } -void CCmpUnitMotion::RequestLongPath(CFixedVector2D from, const ICmpPathfinder::Goal& goal) +void CCmpUnitMotion::RequestLongPath(const CFixedVector2D& from, const PathGoal& goal) { CmpPtr cmpPathfinder(GetSystemEntity()); if (!cmpPathfinder) return; - cmpPathfinder->SetDebugPath(from.X, from.Y, goal, m_PassClass, m_CostClass); + PathGoal improvedGoal = goal; + improvedGoal.maxdist = SHORT_PATH_SEARCH_RANGE / 2; - m_ExpectedPathTicket = cmpPathfinder->ComputePathAsync(from.X, from.Y, goal, m_PassClass, m_CostClass, GetEntityId()); + cmpPathfinder->SetDebugPath(from.X, from.Y, improvedGoal, m_PassClass); + + m_ExpectedPathTicket = cmpPathfinder->ComputePathAsync(from.X, from.Y, improvedGoal, m_PassClass, GetEntityId()); } -void CCmpUnitMotion::RequestShortPath(CFixedVector2D from, const ICmpPathfinder::Goal& goal, bool avoidMovingUnits) +void CCmpUnitMotion::RequestShortPath(const CFixedVector2D &from, const PathGoal& goal, bool avoidMovingUnits) { CmpPtr cmpPathfinder(GetSystemEntity()); if (!cmpPathfinder) @@ -1292,56 +1381,6 @@ void CCmpUnitMotion::RequestShortPath(CFixedVector2D from, const ICmpPathfinder: m_ExpectedPathTicket = cmpPathfinder->ComputeShortPathAsync(from.X, from.Y, m_Radius, SHORT_PATH_SEARCH_RANGE, goal, m_PassClass, avoidMovingUnits, m_TargetEntity, GetEntityId()); } -bool CCmpUnitMotion::PickNextLongWaypoint(const CFixedVector2D& pos, bool avoidMovingUnits) -{ - // If there's no long path, we can't pick the next waypoint from it - if (m_LongPath.m_Waypoints.empty()) - return false; - - // First try to get the immediate next waypoint - entity_pos_t targetX = m_LongPath.m_Waypoints.back().x; - entity_pos_t targetZ = m_LongPath.m_Waypoints.back().z; - m_LongPath.m_Waypoints.pop_back(); - - // To smooth the motion and avoid grid-constrained movement and allow dynamic obstacle avoidance, - // try skipping some more waypoints if they're close enough - - while (!m_LongPath.m_Waypoints.empty()) - { - CFixedVector2D w(m_LongPath.m_Waypoints.back().x, m_LongPath.m_Waypoints.back().z); - if ((w - pos).CompareLength(WAYPOINT_ADVANCE_MAX) > 0) - break; - targetX = m_LongPath.m_Waypoints.back().x; - targetZ = m_LongPath.m_Waypoints.back().z; - m_LongPath.m_Waypoints.pop_back(); - } - - // Now we need to recompute a short path to the waypoint - - ICmpPathfinder::Goal goal; - if (m_LongPath.m_Waypoints.empty()) - { - // This was the last waypoint - head for the exact goal - goal = m_FinalGoal; - } - else - { - // Head for somewhere near the waypoint (but allow some leeway in case it's obstructed) - goal.type = ICmpPathfinder::Goal::CIRCLE; - goal.hw = SHORT_PATH_GOAL_RADIUS; - goal.x = targetX; - goal.z = targetZ; - } - - CmpPtr cmpPathfinder(GetSystemEntity()); - if (!cmpPathfinder) - return false; - - m_ExpectedPathTicket = cmpPathfinder->ComputeShortPathAsync(pos.X, pos.Y, m_Radius, SHORT_PATH_SEARCH_RANGE, goal, m_PassClass, avoidMovingUnits, GetEntityId(), GetEntityId()); - - return true; -} - bool CCmpUnitMotion::MoveToPointRange(entity_pos_t x, entity_pos_t z, entity_pos_t minRange, entity_pos_t maxRange) { return MoveToPointRange(x, z, minRange, maxRange, INVALID_ENTITY); @@ -1357,52 +1396,41 @@ bool CCmpUnitMotion::MoveToPointRange(entity_pos_t x, entity_pos_t z, entity_pos CFixedVector2D pos = cmpPosition->GetPosition2D(); - ICmpPathfinder::Goal goal; + PathGoal goal; + goal.x = x; + goal.z = z; if (minRange.IsZero() && maxRange.IsZero()) { - // Handle the non-ranged mode: + // Non-ranged movement: - // Check whether this point is in an obstruction - - CmpPtr cmpObstructionManager(GetSystemEntity()); - if (!cmpObstructionManager) - return false; - - ICmpObstructionManager::ObstructionSquare obstruction; - if (cmpObstructionManager->FindMostImportantObstruction(GetObstructionFilter(true), x, z, m_Radius, obstruction)) - { - // If we're aiming inside a building, then aim for the outline of the building instead - // TODO: if we're aiming at a unit then maybe a circle would look nicer? - - goal.type = ICmpPathfinder::Goal::SQUARE; - goal.x = obstruction.x; - goal.z = obstruction.z; - goal.u = obstruction.u; - goal.v = obstruction.v; - goal.hw = obstruction.hw + m_Radius + g_GoalDelta; // nudge the goal outwards so it doesn't intersect the building itself - goal.hh = obstruction.hh + m_Radius + g_GoalDelta; - } - else - { - // Unobstructed - head directly for the goal - goal.type = ICmpPathfinder::Goal::POINT; - goal.x = x; - goal.z = z; - } + // Head directly for the goal + goal.type = PathGoal::POINT; } else { + // Ranged movement: + entity_pos_t distance = (pos - CFixedVector2D(x, z)).Length(); - entity_pos_t goalDistance; if (distance < minRange) { - goalDistance = minRange + g_GoalDelta; + // Too close to target - move outwards to a circle + // that's slightly larger than the min range + goal.type = PathGoal::CIRCLE;// TODO: INVERTED_CIRCLE; + goal.hw = minRange + g_GoalDelta; } else if (maxRange >= entity_pos_t::Zero() && distance > maxRange) { - goalDistance = maxRange - g_GoalDelta; + // Too far from target - move inwards to a circle + // that's slightly smaller than the max range + goal.type = PathGoal::CIRCLE; + goal.hw = maxRange - g_GoalDelta; + + // If maxRange was abnormally small, + // collapse the circle into a point + if (goal.hw <= entity_pos_t::Zero()) + goal.type = PathGoal::POINT; } else { @@ -1411,15 +1439,6 @@ bool CCmpUnitMotion::MoveToPointRange(entity_pos_t x, entity_pos_t z, entity_pos FaceTowardsPointFromPos(pos, x, z); return false; } - - // TODO: what happens if goalDistance < 0? (i.e. we probably can never get close enough to the target) - - goal.type = ICmpPathfinder::Goal::CIRCLE; - goal.x = x; - goal.z = z; - - // Formerly added m_Radius, but it seems better to go by the mid-point. - goal.hw = goalDistance; } m_State = STATE_INDIVIDUAL_PATH; @@ -1445,8 +1464,8 @@ bool CCmpUnitMotion::IsInPointRange(entity_pos_t x, entity_pos_t z, entity_pos_t bool hasObstruction = false; CmpPtr cmpObstructionManager(GetSystemEntity()); ICmpObstructionManager::ObstructionSquare obstruction; - if (cmpObstructionManager) - hasObstruction = cmpObstructionManager->FindMostImportantObstruction(GetObstructionFilter(true), x, z, m_Radius, obstruction); +//TODO if (cmpObstructionManager) +// hasObstruction = cmpObstructionManager->FindMostImportantObstruction(GetObstructionFilter(true), x, z, m_Radius, obstruction); if (minRange.IsZero() && maxRange.IsZero() && hasObstruction) { @@ -1469,21 +1488,15 @@ bool CCmpUnitMotion::IsInPointRange(entity_pos_t x, entity_pos_t z, entity_pos_t entity_pos_t distance = (pos - CFixedVector2D(x, z)).Length(); if (distance < minRange) - { return false; - } else if (maxRange >= entity_pos_t::Zero() && distance > maxRange) - { return false; - } else - { return true; - } } } -bool CCmpUnitMotion::ShouldTreatTargetAsCircle(entity_pos_t range, entity_pos_t hw, entity_pos_t hh, entity_pos_t circleRadius) +bool CCmpUnitMotion::ShouldTreatTargetAsCircle(entity_pos_t range, entity_pos_t hw, entity_pos_t hh, entity_pos_t circleRadius) const { // Given a square, plus a target range we should reach, the shape at that distance // is a round-cornered square which we can approximate as either a circle or as a square. @@ -1518,135 +1531,7 @@ bool CCmpUnitMotion::MoveToTargetRange(entity_id_t target, entity_pos_t minRange if (cmpObstruction) hasObstruction = cmpObstruction->GetObstructionSquare(obstruction); - /* - * If we're starting outside the maxRange, we need to move closer in. - * If we're starting inside the minRange, we need to move further out. - * These ranges are measured from the center of this entity to the edge of the target; - * we add the goal range onto the size of the target shape to get the goal shape. - * (Then we extend it outwards/inwards by a little bit to be sure we'll end up - * within the right range, in case of minor numerical inaccuracies.) - * - * There's a bit of a problem with large square targets: - * the pathfinder only lets us move to goals that are squares, but the points an equal - * distance from the target make a rounded square shape instead. - * - * When moving closer, we could shrink the goal radius to 1/sqrt(2) so the goal shape fits entirely - * within the desired rounded square, but that gives an unfair advantage to attackers who approach - * the target diagonally. - * - * If the target is small relative to the range (e.g. archers attacking anything), - * then we cheat and pretend the target is actually a circle. - * (TODO: that probably looks rubbish for things like walls?) - * - * If the target is large relative to the range (e.g. melee units attacking buildings), - * then we multiply maxRange by approx 1/sqrt(2) to guarantee they'll always aim close enough. - * (Those units should set minRange to 0 so they'll never be considered *too* close.) - */ - - if (hasObstruction) - { - CFixedVector2D halfSize(obstruction.hw, obstruction.hh); - ICmpPathfinder::Goal goal; - goal.x = obstruction.x; - goal.z = obstruction.z; - - entity_pos_t distance = Geometry::DistanceToSquare(pos - CFixedVector2D(obstruction.x, obstruction.z), obstruction.u, obstruction.v, halfSize); - - // compare with previous obstruction - ICmpObstructionManager::ObstructionSquare previousObstruction; - cmpObstruction->GetPreviousObstructionSquare(previousObstruction); - entity_pos_t previousDistance = Geometry::DistanceToSquare(pos - CFixedVector2D(previousObstruction.x, previousObstruction.z), obstruction.u, obstruction.v, halfSize); - - if (distance < minRange && previousDistance < minRange) - { - // Too close to the square - need to move away - - // TODO: maybe we should do the ShouldTreatTargetAsCircle thing here? - - entity_pos_t goalDistance = minRange + g_GoalDelta; - - goal.type = ICmpPathfinder::Goal::SQUARE; - goal.u = obstruction.u; - goal.v = obstruction.v; - entity_pos_t delta = std::max(goalDistance, m_Radius + entity_pos_t::FromInt(TERRAIN_TILE_SIZE)/16); // ensure it's far enough to not intersect the building itself - goal.hw = obstruction.hw + delta; - goal.hh = obstruction.hh + delta; - } - else if (maxRange < entity_pos_t::Zero() || distance < maxRange || previousDistance < maxRange) - { - // We're already in range - no need to move anywhere - if (m_FacePointAfterMove) - FaceTowardsPointFromPos(pos, goal.x, goal.z); - return false; - } - else - { - // We might need to move closer: - - // Circumscribe the square - entity_pos_t circleRadius = halfSize.Length(); - - if (ShouldTreatTargetAsCircle(maxRange, obstruction.hw, obstruction.hh, circleRadius)) - { - // The target is small relative to our range, so pretend it's a circle - - // Note that the distance to the circle will always be less than - // the distance to the square, so the previous "distance < maxRange" - // check is still valid (though not sufficient) - entity_pos_t circleDistance = (pos - CFixedVector2D(obstruction.x, obstruction.z)).Length() - circleRadius; - - if (circleDistance < maxRange) - { - // We're already in range - no need to move anywhere - if (m_FacePointAfterMove) - FaceTowardsPointFromPos(pos, goal.x, goal.z); - return false; - } - - entity_pos_t previousCircleDistance = (pos - CFixedVector2D(previousObstruction.x, previousObstruction.z)).Length() - circleRadius; - - if (previousCircleDistance < maxRange) - { - // We're already in range - no need to move anywhere - if (m_FacePointAfterMove) - FaceTowardsPointFromPos(pos, goal.x, goal.z); - return false; - } - - - entity_pos_t goalDistance = maxRange - g_GoalDelta; - - goal.type = ICmpPathfinder::Goal::CIRCLE; - goal.hw = circleRadius + goalDistance; - } - else - { - // The target is large relative to our range, so treat it as a square and - // get close enough that the diagonals come within range - - entity_pos_t goalDistance = (maxRange - g_GoalDelta)*2 / 3; // multiply by slightly less than 1/sqrt(2) - - goal.type = ICmpPathfinder::Goal::SQUARE; - goal.u = obstruction.u; - goal.v = obstruction.v; - entity_pos_t delta = std::max(goalDistance, m_Radius + entity_pos_t::FromInt(TERRAIN_TILE_SIZE)/16); // ensure it's far enough to not intersect the building itself - goal.hw = obstruction.hw + delta; - goal.hh = obstruction.hh + delta; - } - } - - m_State = STATE_INDIVIDUAL_PATH; - m_TargetEntity = target; - m_TargetOffset = CFixedVector2D(); - m_TargetMinRange = minRange; - m_TargetMaxRange = maxRange; - m_FinalGoal = goal; - - BeginPathing(pos, goal); - - return true; - } - else + if (!hasObstruction) { // The target didn't have an obstruction or obstruction shape, so treat it as a point instead @@ -1656,8 +1541,117 @@ bool CCmpUnitMotion::MoveToTargetRange(entity_id_t target, entity_pos_t minRange CFixedVector2D targetPos = cmpTargetPosition->GetPosition2D(); - return MoveToPointRange(targetPos.X, targetPos.Y, minRange, maxRange, target); + return MoveToPointRange(targetPos.X, targetPos.Y, minRange, maxRange); } + + /* + * If we're starting outside the maxRange, we need to move closer in. + * If we're starting inside the minRange, we need to move further out. + * These ranges are measured from the center of this entity to the edge of the target; + * we add the goal range onto the size of the target shape to get the goal shape. + * (Then we extend it outwards/inwards by a little bit to be sure we'll end up + * within the right range, in case of minor numerical inaccuracies.) + * + * There's a bit of a problem with large square targets: + * the pathfinder only lets us move to goals that are squares, but the points an equal + * distance from the target make a rounded square shape instead. + * + * When moving closer, we could shrink the goal radius to 1/sqrt(2) so the goal shape fits entirely + * within the desired rounded square, but that gives an unfair advantage to attackers who approach + * the target diagonally. + * + * If the target is small relative to the range (e.g. archers attacking anything), + * then we cheat and pretend the target is actually a circle. + * (TODO: that probably looks rubbish for things like walls?) + * + * If the target is large relative to the range (e.g. melee units attacking buildings), + * then we multiply maxRange by approx 1/sqrt(2) to guarantee they'll always aim close enough. + * (Those units should set minRange to 0 so they'll never be considered *too* close.) + */ + + CFixedVector2D halfSize(obstruction.hw, obstruction.hh); + PathGoal goal; + goal.x = obstruction.x; + goal.z = obstruction.z; + + entity_pos_t distance = Geometry::DistanceToSquare(pos - CFixedVector2D(obstruction.x, obstruction.z), obstruction.u, obstruction.v, halfSize); + + if (distance < minRange) + { + // Too close to the square - need to move away + + // TODO: maybe we should do the ShouldTreatTargetAsCircle thing here? + + entity_pos_t goalDistance = minRange + g_GoalDelta; + + goal.type = PathGoal::SQUARE; + goal.u = obstruction.u; + goal.v = obstruction.v; + entity_pos_t delta = std::max(goalDistance, m_Radius + entity_pos_t::FromInt(TERRAIN_TILE_SIZE)/16); // ensure it's far enough to not intersect the building itself + goal.hw = obstruction.hw + delta; + goal.hh = obstruction.hh + delta; + } + else if (maxRange < entity_pos_t::Zero() || distance < maxRange) + { + // We're already in range - no need to move anywhere + FaceTowardsPointFromPos(pos, goal.x, goal.z); + return false; + } + else + { + // We might need to move closer: + + // Circumscribe the square + entity_pos_t circleRadius = halfSize.Length(); + + if (ShouldTreatTargetAsCircle(maxRange, obstruction.hw, obstruction.hh, circleRadius)) + { + // The target is small relative to our range, so pretend it's a circle + + // Note that the distance to the circle will always be less than + // the distance to the square, so the previous "distance < maxRange" + // check is still valid (though not sufficient) + entity_pos_t circleDistance = (pos - CFixedVector2D(obstruction.x, obstruction.z)).Length() - circleRadius; + + if (circleDistance < maxRange) + { + // We're already in range - no need to move anywhere + if (m_FacePointAfterMove) + FaceTowardsPointFromPos(pos, goal.x, goal.z); + return false; + } + + entity_pos_t goalDistance = maxRange - g_GoalDelta; + + goal.type = PathGoal::CIRCLE; + goal.hw = circleRadius + goalDistance; + } + else + { + // The target is large relative to our range, so treat it as a square and + // get close enough that the diagonals come within range + + entity_pos_t goalDistance = (maxRange - g_GoalDelta)*2 / 3; // multiply by slightly less than 1/sqrt(2) + + goal.type = PathGoal::SQUARE; + goal.u = obstruction.u; + goal.v = obstruction.v; + entity_pos_t delta = std::max(goalDistance, m_Radius + entity_pos_t::FromInt(TERRAIN_TILE_SIZE)/16); // ensure it's far enough to not intersect the building itself + goal.hw = obstruction.hw + delta; + goal.hh = obstruction.hh + delta; + } + } + + m_State = STATE_INDIVIDUAL_PATH; + m_TargetEntity = target; + m_TargetOffset = CFixedVector2D(); + m_TargetMinRange = minRange; + m_TargetMaxRange = maxRange; + m_FinalGoal = goal; + + BeginPathing(pos, goal); + + return true; } bool CCmpUnitMotion::IsInTargetRange(entity_id_t target, entity_pos_t minRange, entity_pos_t maxRange) @@ -1742,8 +1736,8 @@ void CCmpUnitMotion::MoveToFormationOffset(entity_id_t target, entity_pos_t x, e CFixedVector2D pos = cmpPosition->GetPosition2D(); - ICmpPathfinder::Goal goal; - goal.type = ICmpPathfinder::Goal::POINT; + PathGoal goal; + goal.type = PathGoal::POINT; goal.x = pos.X; goal.z = pos.Y; @@ -1761,7 +1755,7 @@ void CCmpUnitMotion::MoveToFormationOffset(entity_id_t target, entity_pos_t x, e -void CCmpUnitMotion::RenderPath(const ICmpPathfinder::Path& path, std::vector& lines, CColor color) +void CCmpUnitMotion::RenderPath(const WaypointPath& path, std::vector& lines, CColor color) { bool floating = false; CmpPtr cmpPosition(GetEntityHandle()); diff --git a/source/simulation2/components/ICmpObstruction.h b/source/simulation2/components/ICmpObstruction.h index 1b289da8dc..6d87d28634 100644 --- a/source/simulation2/components/ICmpObstruction.h +++ b/source/simulation2/components/ICmpObstruction.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2013 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -56,6 +56,8 @@ public: virtual entity_pos_t GetUnitRadius() = 0; + virtual void SetUnitClearance(const entity_pos_t& clearance) = 0; + virtual bool IsControlPersistent() = 0; /** diff --git a/source/simulation2/components/ICmpObstructionManager.h b/source/simulation2/components/ICmpObstructionManager.h index 58afdc69f6..e4748e4e15 100644 --- a/source/simulation2/components/ICmpObstructionManager.h +++ b/source/simulation2/components/ICmpObstructionManager.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2012 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -48,7 +48,7 @@ class IObstructionTestFilter; * Units can be marked as either moving or stationary, which simply determines whether * certain filters include or exclude them. * - * The @c Rasterise function approximates the current set of shapes onto a 2D grid, + * The @c Rasterize function approximates the current set of shapes onto a 2D grid, * for use with tile-based pathfinding. */ class ICmpObstructionManager : public IComponent @@ -114,14 +114,15 @@ public: * @param ent entity ID associated with this shape (or INVALID_ENTITY if none) * @param x,z coordinates of center, in world space * @param r radius of circle or half the unit's width/height + * @param clearance pathfinding clearance of the unit * @param flags a set of EFlags values * @param group control group (typically the owner entity, or a formation controller entity * - units ignore collisions with others in the same group) * @return a valid tag for manipulating the shape * @see UnitShape */ - virtual tag_t AddUnitShape(entity_id_t ent, entity_pos_t x, entity_pos_t z, entity_angle_t r, flags_t flags, - entity_id_t group) = 0; + virtual tag_t AddUnitShape(entity_id_t ent, entity_pos_t x, entity_pos_t z, entity_angle_t r, entity_pos_t clearance, + flags_t flags, entity_id_t group) = 0; /** * Adjust the position and angle of an existing shape. @@ -205,7 +206,7 @@ public: std::vector* out) = 0; /** - * Bit-flags for Rasterise. + * Bit-flags for Rasterize. */ enum TileObstruction { @@ -215,16 +216,20 @@ public: }; /** - * Convert the current set of shapes onto a grid. - * Tiles that are intersected by a pathfind-blocking shape will have TILE_OBSTRUCTED_PATHFINDING set; - * tiles that are intersected by a foundation-blocking shape will also have TILE_OBSTRUCTED_FOUNDATION; - * tiles that are outside the world bounds will also have TILE_OUTOFBOUNDS; - * others will be set to 0. - * This is very cheap if the grid has been rasterised before and the set of shapes has not changed. - * @param grid the grid to be updated - * @return true if any changes were made to the grid, false if it was already up-to-date + * Convert the current set of shapes onto a navcell grid. + * Shapes are expanded by the clearance radius @p expand. + * Only shapes with at least one of the flags from @p requireMask will be considered. + * @p setMask will be ORed onto the @p grid value for all navcells + * that are wholly enclosed by an expanded shape. */ - virtual bool Rasterise(Grid& grid) = 0; + virtual void Rasterize(Grid& grid, const entity_pos_t& expand, ICmpObstructionManager::flags_t requireMask, u16 setMask) = 0; + + /** + * Gets dirtiness information and resets it afterwards. Then it's the role of CCmpPathfinder + * to pass the information to other components if needed. (AIs, etc.) + * The return value is false if an update is unnecessary. + */ + virtual bool GetDirtinessData(Grid& dirtinessGrid, bool& globalUpdateNeeded) = 0; /** * Standard representation for all types of shapes, for use with geometry processing code. @@ -248,12 +253,10 @@ public: 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) = 0; /** - * Find a single obstruction that blocks a unit at the given point with the given radius. - * Static obstructions (buildings) are more important than unit obstructions, and - * obstructions that cover the given point are more important than those that only cover - * the point expanded by the radius. + * Returns the entity IDs of all unit shapes that intersect the given + * obstruction square, filtering out using the given filter. */ - virtual bool FindMostImportantObstruction(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, ObstructionSquare& square) = 0; + virtual void GetUnitsOnObstruction(const ObstructionSquare& square, std::vector& out, const IObstructionTestFilter& filter) = 0; /** * Get the obstruction square representing the given shape. @@ -270,6 +273,8 @@ public: */ virtual void SetPassabilityCircular(bool enabled) = 0; + virtual bool GetPassabilityCircular() const = 0; + /** * Toggle the rendering of debug info. */ @@ -344,10 +349,19 @@ public: { if (group == m_Group || (group2 != INVALID_ENTITY && group2 == m_Group)) return false; + + // If an obstruction already blocks tile-based pathfinding, + // it will be handled as part of the terrain passability handling + // and doesn't need to be matched by this filter + if (flags & ICmpObstructionManager::FLAG_BLOCK_PATHFINDING) + return false; + if (!(flags & ICmpObstructionManager::FLAG_BLOCK_MOVEMENT)) return false; + if ((flags & ICmpObstructionManager::FLAG_MOVING) && !m_AvoidMoving) return false; + return true; } }; diff --git a/source/simulation2/components/ICmpPathfinder.cpp b/source/simulation2/components/ICmpPathfinder.cpp index 41d0fb04e6..df01d65697 100644 --- a/source/simulation2/components/ICmpPathfinder.cpp +++ b/source/simulation2/components/ICmpPathfinder.cpp @@ -23,4 +23,5 @@ BEGIN_INTERFACE_WRAPPER(Pathfinder) DEFINE_INTERFACE_METHOD_1("SetDebugOverlay", void, ICmpPathfinder, SetDebugOverlay, bool) +DEFINE_INTERFACE_METHOD_1("SetHierDebugOverlay", void, ICmpPathfinder, SetHierDebugOverlay, bool) END_INTERFACE_WRAPPER(Pathfinder) diff --git a/source/simulation2/components/ICmpPathfinder.h b/source/simulation2/components/ICmpPathfinder.h index a91cfdadbd..0ad964f262 100644 --- a/source/simulation2/components/ICmpPathfinder.h +++ b/source/simulation2/components/ICmpPathfinder.h @@ -21,7 +21,8 @@ #include "simulation2/system/Interface.h" #include "simulation2/components/ICmpObstruction.h" -#include "simulation2/helpers/Position.h" +#include "simulation2/helpers/PathGoal.h" +#include "simulation2/helpers/Pathfinding.h" #include "maths/FixedVector2D.h" @@ -39,7 +40,7 @@ template class Grid; * accounts for terrain costs but ignore units, and a 'short' vertex-based pathfinder that * provides precise paths and avoids other units. * - * Both use the same concept of a Goal: either a point, circle or square. + * Both use the same concept of a PathGoal: either a point, circle or square. * (If the starting point is inside the goal shape then the path will move outwards * to reach the shape's outline.) * @@ -48,34 +49,6 @@ template class Grid; class ICmpPathfinder : public IComponent { public: - typedef u16 pass_class_t; - typedef u8 cost_class_t; - - struct Goal - { - enum Type { - POINT, - CIRCLE, - SQUARE - } type; - entity_pos_t x, z; // position of center - CFixedVector2D u, v; // if SQUARE, then orthogonal unit axes - entity_pos_t hw, hh; // if SQUARE, then half width & height; if CIRCLE, then hw is radius - }; - - struct Waypoint - { - entity_pos_t x, z; - }; - - /** - * Returned path. - * Waypoints are in *reverse* order (the earliest is at the back of the list) - */ - struct Path - { - std::vector m_Waypoints; - }; /** * Get the list of all known passability classes. @@ -88,14 +61,21 @@ public: */ virtual pass_class_t GetPassabilityClass(const std::string& name) = 0; + virtual entity_pos_t GetClearance(pass_class_t passClass) const = 0; + /** - * Get the tag for a given movement cost class name. - * Logs an error and returns something acceptable if the name is unrecognised. + * Get the larger clearance in all passability classes. */ - virtual cost_class_t GetCostClass(const std::string& name) = 0; + virtual entity_pos_t GetMaximumClearance() const = 0; virtual const Grid& GetPassabilityGrid() = 0; + /** + * Passes the lazily-stored dirtiness data collected from + * the obstruction manager during the previous grid update. + */ + virtual bool GetDirtinessData(Grid& dirtinessGrid, bool& globalUpdateNeeded) = 0; + /** * Get a grid representing the distance to the shore of the terrain tile. */ @@ -106,7 +86,7 @@ public: * The waypoints correspond to the centers of horizontally/vertically adjacent tiles * along the path. */ - virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, Path& ret) = 0; + virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) = 0; /** * Asynchronous version of ComputePath. @@ -114,12 +94,12 @@ public: * Returns a unique non-zero number, which will match the 'ticket' in the result, * so callers can recognise each individual request they make. */ - virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass, entity_id_t notify) = 0; + virtual u32 ComputePathAsync(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, entity_id_t notify) = 0; /** * If the debug overlay is enabled, render the path that will computed by ComputePath. */ - virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass) = 0; + virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) = 0; /** * Compute a precise path from the given point to the goal, and return the set of waypoints. @@ -127,7 +107,7 @@ public: * a unit of radius 'r' will be able to follow the path with no collisions. * The path is restricted to a box of radius 'range' from the starting point. */ - virtual void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const Goal& goal, pass_class_t passClass, Path& ret) = 0; + virtual void ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) = 0; /** * Asynchronous version of ComputeShortPath (using ControlGroupObstructionFilter). @@ -135,18 +115,7 @@ public: * Returns a unique non-zero number, which will match the 'ticket' in the result, * so callers can recognise each individual request they make. */ - virtual u32 ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const Goal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t group, entity_id_t notify) = 0; - - /** - * Find the speed factor (typically around 1.0) for a unit of the given cost class - * at the given position. - */ - virtual fixed GetMovementSpeed(entity_pos_t x0, entity_pos_t z0, cost_class_t costClass) = 0; - - /** - * Returns the coordinates of the point on the goal that is closest to pos in a straight line. - */ - virtual CFixedVector2D GetNearestPointOnGoal(CFixedVector2D pos, const Goal& goal) = 0; + virtual u32 ComputeShortPathAsync(entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const PathGoal& goal, pass_class_t passClass, bool avoidMovingUnits, entity_id_t group, entity_id_t notify) = 0; /** * Check whether the given movement line is valid and doesn't hit any obstructions @@ -155,14 +124,6 @@ public: */ virtual bool CheckMovement(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, pass_class_t passClass) = 0; - /** - * Check whether a unit placed here is valid and doesn't hit any obstructions - * or impassable terrain. - * @return ICmpObstruction::FOUNDATION_CHECK_SUCCESS if the placement is okay, else - * a value describing the type of failure. - */ - virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass) = 0; - /** * Check whether a unit placed here is valid and doesn't hit any obstructions * or impassable terrain. @@ -170,7 +131,7 @@ public: * @return ICmpObstruction::FOUNDATION_CHECK_SUCCESS if the placement is okay, else * a value describing the type of failure. */ - virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint) = 0; + virtual ICmpObstruction::EFoundationCheck CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass, bool onlyCenterPoint = false) = 0; /** * Check whether a building placed here is valid and doesn't hit any obstructions @@ -195,6 +156,11 @@ public: */ virtual void SetDebugOverlay(bool enabled) = 0; + /** + * Toggle the storage and rendering of debug info for the hierarchical pathfinder. + */ + virtual void SetHierDebugOverlay(bool enabled) = 0; + /** * Finish computing asynchronous path requests and send the CMessagePathResult messages. */ @@ -205,6 +171,16 @@ public: */ virtual void ProcessSameTurnMoves() = 0; + /** + * Regenerates the grid based on the current obstruction list, if necessary + */ + virtual void UpdateGrid() = 0; + + /** + * Returns some stats about the last ComputePath. + */ + virtual void GetDebugData(u32& steps, double& time, Grid& grid) = 0; + DECLARE_INTERFACE_TYPE(Pathfinder) }; diff --git a/source/simulation2/components/ICmpTerritoryManager.h b/source/simulation2/components/ICmpTerritoryManager.h index b1b23deb5a..b03e55819b 100644 --- a/source/simulation2/components/ICmpTerritoryManager.h +++ b/source/simulation2/components/ICmpTerritoryManager.h @@ -29,6 +29,14 @@ class ICmpTerritoryManager : public IComponent public: virtual bool NeedUpdate(size_t* dirtyID) = 0; + /** + * Number of pathfinder navcells per territory tile. + * Passability data is stored per navcell, but we probably don't need that much + * resolution, and a lower resolution can make the boundary lines look prettier + * and will take less memory, so we downsample the passability data. + */ + static const int NAVCELLS_PER_TERRITORY_TILE = 8; + static const int TERRITORY_PLAYER_MASK = 0x1F; static const int TERRITORY_CONNECTED_MASK = 0x20; static const int TERRITORY_BLINKING_MASK = 0x40; diff --git a/source/simulation2/components/ICmpUnitMotion.cpp b/source/simulation2/components/ICmpUnitMotion.cpp index 7f33dd4312..a4c6b4a120 100644 --- a/source/simulation2/components/ICmpUnitMotion.cpp +++ b/source/simulation2/components/ICmpUnitMotion.cpp @@ -112,9 +112,9 @@ public: m_Script.CallVoid("SetFacePointAfterMove", facePointAfterMove); } - virtual ICmpPathfinder::pass_class_t GetPassabilityClass() + virtual pass_class_t GetPassabilityClass() { - return m_Script.Call("GetPassabilityClass"); + return m_Script.Call("GetPassabilityClass"); } virtual std::string GetPassabilityClassName() diff --git a/source/simulation2/components/ICmpUnitMotion.h b/source/simulation2/components/ICmpUnitMotion.h index 52274d66a0..8989c3127b 100644 --- a/source/simulation2/components/ICmpUnitMotion.h +++ b/source/simulation2/components/ICmpUnitMotion.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2011 Wildfire Games. +/* Copyright (C) 2012 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -27,8 +27,7 @@ * Motion interface for entities with complex movement capabilities. * (Simpler motion is handled by ICmpMotion instead.) * - * Currently this is limited to telling the entity to walk to a point. - * Eventually it should support different movement speeds, moving to areas + * It should eventually support different movement speeds, moving to areas * instead of points, moving as part of a group, moving as part of a formation, * etc. */ @@ -38,6 +37,7 @@ public: /** * Attempt to walk into range of a to a given point, or as close as possible. + * The range is measured from the center of the unit. * If the unit is already in range, or cannot move anywhere at all, or if there is * some other error, then returns false. * Otherwise, returns true and sends a MotionChanged message after starting to move, @@ -60,6 +60,8 @@ public: /** * Attempt to walk into range of a given target entity, or as close as possible. + * The range is measured between approximately the edges of the unit and the target, so that + * maxRange=0 is not unreachably close to the target. * If the unit is already in range, or cannot move anywhere at all, or if there is * some other error, then returns false. * Otherwise, returns true and sends a MotionChanged message after starting to move, @@ -117,7 +119,7 @@ public: /** * Get the unit's passability class. */ - virtual ICmpPathfinder::pass_class_t GetPassabilityClass() = 0; + virtual pass_class_t GetPassabilityClass() = 0; /** * Get the passability class name (as defined in pathfinder.xml) diff --git a/source/simulation2/components/tests/test_ObstructionManager.h b/source/simulation2/components/tests/test_ObstructionManager.h index ecbfb91212..4608d9ce63 100644 --- a/source/simulation2/components/tests/test_ObstructionManager.h +++ b/source/simulation2/components/tests/test_ObstructionManager.h @@ -26,10 +26,10 @@ class TestCmpObstructionManager : public CxxTest::TestSuite // some variables for setting up a scene with 3 shapes entity_id_t ent1, ent2, ent3; // entity IDs - entity_angle_t ent1a, ent2r, ent3r; // angles/radiuses + entity_angle_t ent1a; // angles entity_pos_t ent1x, ent1z, ent1w, ent1h, // positions/dimensions - ent2x, ent2z, - ent3x, ent3z; + ent2x, ent2z, ent2r, ent2c, + ent3x, ent3z, ent3r, ent3c; entity_id_t ent1g1, ent1g2, ent2g, ent3g; // control groups tag_t shape1, shape2, shape3; @@ -58,12 +58,14 @@ public: ent2 = 2; ent2r = fixed::FromFloat(1); + ent2c = fixed::Zero(); ent2x = ent1x; ent2z = ent1z; ent2g = ent1g1; ent3 = 3; ent3r = fixed::FromFloat(3); + ent3c = fixed::Zero(); ent3x = ent2x; ent3z = ent2z + ent2r + ent3r; // ensure it just touches the border of ent2 ent3g = ent3; @@ -77,11 +79,11 @@ public: ICmpObstructionManager::FLAG_BLOCK_MOVEMENT | ICmpObstructionManager::FLAG_MOVING, ent1g1, ent1g2); - shape2 = cmp->AddUnitShape(ent2, ent2x, ent2z, ent2r, + shape2 = cmp->AddUnitShape(ent2, ent2x, ent2z, ent2r, ent2c, ICmpObstructionManager::FLAG_BLOCK_CONSTRUCTION | ICmpObstructionManager::FLAG_BLOCK_FOUNDATION, ent2g); - shape3 = cmp->AddUnitShape(ent3, ent3x, ent3z, ent3r, + shape3 = cmp->AddUnitShape(ent3, ent3x, ent3z, ent3r, ent3c, ICmpObstructionManager::FLAG_BLOCK_MOVEMENT | ICmpObstructionManager::FLAG_BLOCK_FOUNDATION, ent3g); } diff --git a/source/simulation2/components/tests/test_Pathfinder.h b/source/simulation2/components/tests/test_Pathfinder.h index b3d381565c..822de215d6 100644 --- a/source/simulation2/components/tests/test_Pathfinder.h +++ b/source/simulation2/components/tests/test_Pathfinder.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2013 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -26,6 +26,7 @@ #include "lib/timer.h" #include "lib/tex/tex.h" #include "ps/Loader.h" +#include "ps/Pyrogenesis.h" #include "simulation2/Simulation2.h" class TestCmpPathfinder : public CxxTest::TestSuite @@ -33,11 +34,12 @@ class TestCmpPathfinder : public CxxTest::TestSuite public: void setUp() { - CXeromyces::Startup(); - g_VFS = CreateVfs(20 * MiB); - TS_ASSERT_OK(g_VFS->Mount(L"", DataDir()/"mods"/"public", VFS_MOUNT_MUST_EXIST)); - TS_ASSERT_OK(g_VFS->Mount(L"cache/", DataDir()/"cache")); + TS_ASSERT_OK(g_VFS->Mount(L"", DataDir() / "mods" / "mod", VFS_MOUNT_MUST_EXIST)); + TS_ASSERT_OK(g_VFS->Mount(L"", DataDir() / "mods" / "public", VFS_MOUNT_MUST_EXIST, 1)); + TS_ASSERT_OK(g_VFS->Mount(L"cache/", DataDir() / "cache")); + + CXeromyces::Startup(); // Need some stuff for terrain movement costs: // (TODO: this ought to be independent of any graphics code) @@ -54,7 +56,6 @@ public: CXeromyces::Terminate(); } - // disabled by default; run tests with the "-test TestCmpPathfinder" flag to enable void test_performance_DISABLED() { CTerrain terrain; @@ -84,8 +85,8 @@ public: entity_pos_t z1 = entity_pos_t::FromInt(495); ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, x1, z1 }; - ICmpPathfinder::Path path; - cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), cmp->GetCostClass("default"), path); + WaypointPath path; + cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), path); for (size_t i = 0; i < path.m_Waypoints.size(); ++i) printf("%d: %f %f\n", (int)i, path.m_Waypoints[i].x.ToDouble(), path.m_Waypoints[i].z.ToDouble()); #endif @@ -99,10 +100,10 @@ public: entity_pos_t z0 = entity_pos_t::FromInt(rand() % 512); entity_pos_t x1 = x0 + entity_pos_t::FromInt(rand() % 64); entity_pos_t z1 = z0 + entity_pos_t::FromInt(rand() % 64); - ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, x1, z1 }; + PathGoal goal = { PathGoal::POINT, x1, z1 }; - ICmpPathfinder::Path path; - cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), cmp->GetCostClass("default"), path); + WaypointPath path; + cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), path); } t = timer_Time() - t; @@ -129,14 +130,216 @@ public: fixed x = fixed::FromFloat(1.5f*range.ToFloat() * rand()/(float)RAND_MAX); fixed z = fixed::FromFloat(1.5f*range.ToFloat() * rand()/(float)RAND_MAX); // printf("# %f %f\n", x.ToFloat(), z.ToFloat()); - cmpObstructionMan->AddUnitShape(INVALID_ENTITY, x, z, fixed::FromInt(2), 0, INVALID_ENTITY); + cmpObstructionMan->AddUnitShape(INVALID_ENTITY, x, z, fixed::FromInt(2), fixed::Zero(), 0, INVALID_ENTITY); } NullObstructionFilter filter; - ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, range, range }; - ICmpPathfinder::Path path; + PathGoal goal = { PathGoal::POINT, range, range }; + WaypointPath path; cmpPathfinder->ComputeShortPath(filter, range/3, range/3, fixed::FromInt(2), range, goal, 0, path); for (size_t i = 0; i < path.m_Waypoints.size(); ++i) printf("# %d: %f %f\n", (int)i, path.m_Waypoints[i].x.ToFloat(), path.m_Waypoints[i].z.ToFloat()); } + + template + void DumpGrid(std::ostream& stream, const Grid& grid, int mask) + { + for (u16 j = 0; j < grid.m_H; ++j) + { + for (u16 i = 0; i < grid.m_W; ) + { + if (!(grid.get(i, j) & mask)) + { + i++; + continue; + } + + u16 i0 = i; + for (i = i0+1; ; ++i) + { + if (i >= grid.m_W || !(grid.get(i, j) & mask)) + { + stream << " \n"; + break; + } + } + } + } + } + + void test_perf2_DISABLED() + { + CTerrain terrain; + + CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); + sim2.LoadDefaultScripts(); + sim2.ResetState(); + + CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself + + LDR_BeginRegistering(); + mapReader->LoadMap(L"maps/scenarios/Peloponnese.pmp", + sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue, + &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL, + &sim2, &sim2.GetSimContext(), -1, false); + LDR_EndRegistering(); + TS_ASSERT_OK(LDR_NonprogressiveLoad()); + + sim2.Update(0); + + std::ofstream stream(OsString("perf2.html").c_str(), std::ofstream::out | std::ofstream::trunc); + + CmpPtr cmpObstructionManager(sim2, SYSTEM_ENTITY); + CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY); + + pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default") | ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING; + const Grid& obstructions = cmpPathfinder->GetPassabilityGrid(); + + int scale = 1; + stream << "\n"; + stream << "\n"; + stream << "\n"; + stream << "\n"; + + stream << " \n"; + DumpGrid(stream, obstructions, obstructionsMask); + stream << " \n"; + + DumpPath(stream, 128*4, 256*4, 128*4, 384*4, cmpPathfinder); +// RepeatPath(500, 128*4, 256*4, 128*4, 384*4, cmpPathfinder); +// +// DumpPath(stream, 128*4, 204*4, 192*4, 204*4, cmpPathfinder); +// +// DumpPath(stream, 128*4, 230*4, 32*4, 230*4, cmpPathfinder); + + stream << "\n"; + stream << "\n"; + } + + void test_perf3_DISABLED() + { + CTerrain terrain; + + CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); + sim2.LoadDefaultScripts(); + sim2.ResetState(); + + CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself + + LDR_BeginRegistering(); + mapReader->LoadMap(L"maps/scenarios/Peloponnese.pmp", + sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue, + &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL, + &sim2, &sim2.GetSimContext(), -1, false); + LDR_EndRegistering(); + TS_ASSERT_OK(LDR_NonprogressiveLoad()); + + sim2.Update(0); + + std::ofstream stream(OsString("perf3.html").c_str(), std::ofstream::out | std::ofstream::trunc); + + CmpPtr cmpObstructionManager(sim2, SYSTEM_ENTITY); + CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY); + + pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default") | ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING; + const Grid& obstructions = cmpPathfinder->GetPassabilityGrid(); + + int scale = 31; + stream << "\n"; + stream << "\n"; + stream << "\n"; + stream << "\n"; + stream << "\n"; + stream << "\n"; + stream << "\n"; + + stream << " \n"; + DumpGrid(stream, obstructions, obstructionsMask); + stream << " \n"; + + for (int j = 160; j < 190; ++j) + for (int i = 220; i < 290; ++i) + DumpPath(stream, 230, 178, i, j, cmpPathfinder); + + stream << "\n"; + stream << "\n"; + stream << "\n"; + } + + void DumpPath(std::ostream& stream, int i0, int j0, int i1, int j1, CmpPtr& cmpPathfinder) + { + entity_pos_t x0 = entity_pos_t::FromInt(i0); + entity_pos_t z0 = entity_pos_t::FromInt(j0); + entity_pos_t x1 = entity_pos_t::FromInt(i1); + entity_pos_t z1 = entity_pos_t::FromInt(j1); + + PathGoal goal = { PathGoal::POINT, x1, z1 }; + + WaypointPath path; + cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path); + + u32 debugSteps; + double debugTime; + Grid debugGrid; + cmpPathfinder->GetDebugData(debugSteps, debugTime, debugGrid); +// stream << " \n"; + + stream << " \n"; +// stream << " \n"; +// DumpGrid(stream, debugGrid, 1); +// stream << " \n"; +// stream << " \n"; +// DumpGrid(stream, debugGrid, 2); +// stream << " \n"; +// stream << " \n"; +// DumpGrid(stream, debugGrid, 3); +// stream << " \n"; + stream << " \n"; + + stream << " \n"; + } + + void RepeatPath(int n, int i0, int j0, int i1, int j1, CmpPtr& cmpPathfinder) + { + entity_pos_t x0 = entity_pos_t::FromInt(i0); + entity_pos_t z0 = entity_pos_t::FromInt(j0); + entity_pos_t x1 = entity_pos_t::FromInt(i1); + entity_pos_t z1 = entity_pos_t::FromInt(j1); + + PathGoal goal = { PathGoal::POINT, x1, z1 }; + + double t = timer_Time(); + for (int i = 0; i < n; ++i) + { + WaypointPath path; + cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path); + } + t = timer_Time() - t; + debug_printf("### RepeatPath %fms each (%fs total)\n", 1000*t / n, t); + } + }; diff --git a/source/simulation2/components/tests/test_TerritoryManager.h b/source/simulation2/components/tests/test_TerritoryManager.h index 4846ddd4f3..4e7273acac 100644 --- a/source/simulation2/components/tests/test_TerritoryManager.h +++ b/source/simulation2/components/tests/test_TerritoryManager.h @@ -49,11 +49,11 @@ public: TS_ASSERT_EQUALS((player_id_t)7, boundaries[0].owner); TS_ASSERT_EQUALS(false, boundaries[0].blinking); // high bits aren't set by GetGrid - // assumes CELL_SIZE is 4; dealt with in TestBoundaryPointsEqual - int expectedPoints[][2] = {{ 2, 4}, { 6, 4}, {10, 4}, {14, 4}, {18, 4}, {22, 4}, - {24, 6}, {24,10}, {24,14}, - {22,16}, {18,16}, {14,16}, {10,16}, { 6,16}, { 2,16}, - { 0,14}, { 0,10}, { 0, 6}}; + // assumes CELL_SIZE is 2; dealt with in TestBoundaryPointsEqual + int expectedPoints[][2] = {{ 4, 8}, {12, 8}, {20, 8}, {28, 8}, {36, 8}, {44, 8}, + {48,12}, {48,20}, {48,28}, + {44,32}, {36,32}, {28,32}, {20,32}, {12,32}, { 4,32}, + { 0,28}, { 0,20}, { 0,12}}; TestBoundaryPointsEqual(boundaries[0].points, expectedPoints); } @@ -97,8 +97,8 @@ public: } else { - TS_ASSERT_EQUALS(onesInnerNumExpectedPoints, boundary.points.size()); // all inner boundaries are of size 4 - if (boundary.points[0].X < 14.f) + TS_ASSERT_EQUALS(onesInnerNumExpectedPoints, boundary.points.size()); // all inner boundaries are of size 2 + if (boundary.points[0].X < 24.f) { // leftmost inner boundary, i.e. onesInner0 TSM_ASSERT_EQUALS("Found multiple leftmost inner boundaries for territory owned by player 1", onesInner0, (STerritoryBoundary*) NULL); @@ -139,14 +139,14 @@ public: TS_ASSERT_EQUALS(twosOuter->points.size(), 4U); TS_ASSERT_EQUALS(threesOuter->points.size(), 4U); - int onesOuterExpectedPoints[][2] = {{6,4}, {10,4}, {14,4}, {18,4}, {22,4}, {26,4}, - {28,6}, {26,8}, {24,10}, {26,12}, {28,14}, - {26,16}, {22,16}, {18,16}, {14,16}, {10,16}, {6,16}, - {4,14}, {4,10}, {4,6}}; - int onesInner0ExpectedPoints[][2] = {{10,12}, {12,10}, {10,8}, {8,10}}; - int onesInner2ExpectedPoints[][2] = {{18,12}, {20,10}, {18,8}, {16,10}}; - int twosOuterExpectedPoints[][2] = {{18,8}, {20,10}, {18,12}, {16,10}}; - int threesOuterExpectedPoints[][2] = {{26,8}, {28,10}, {26,12}, {24,10}}; + int onesOuterExpectedPoints[][2] = {{12, 8}, {20, 8}, {28, 8}, {36, 8}, {44, 8}, {52, 8}, + {56,12}, {52,16}, {48,20}, {52,24}, {56,28}, + {52,32}, {44,32}, {36,32}, {28,32}, {20,32}, {12,32}, + { 8,28}, { 8,20}, { 8,12}}; + int onesInner0ExpectedPoints[][2] = {{20,24}, {24,20}, {20,16}, {16,20}}; + int onesInner2ExpectedPoints[][2] = {{36,24}, {40,20}, {36,16}, {32,20}}; + int twosOuterExpectedPoints[][2] = {{36,16}, {40,20}, {36,24}, {32,20}}; + int threesOuterExpectedPoints[][2] = {{52,16}, {56,20}, {52,24}, {48,20}}; TestBoundaryPointsEqual(onesOuter->points, onesOuterExpectedPoints); TestBoundaryPointsEqual(onesInner0->points, onesInner0ExpectedPoints); @@ -230,15 +230,15 @@ public: twosInner = tmp; } - int onesOuterExpectedPoints[][2] = {{14, 8}, {18, 8}, {20,10}, {20,14}, {18,16}, {14,16}, {12,14}, {12,10}}; - int twosOuterExpectedPoints[][2] = {{ 6, 0}, {10, 0}, {14, 0}, {16, 2}, {18, 4}, {22, 4}, - {24, 6}, {24,10}, {24,14}, {24,18}, {24,22}, - {22,24}, {18,24}, {14,24}, {10,24}, { 6,24}, - {4, 22}, {4, 18}, {4, 14}, {4, 10}, { 4, 6}, { 4, 2}}; - int twosInnerExpectedPoints[][2] = {{10,20}, {14,20}, {18,20}, {20,18}, {20,14}, {20,10}, {18, 8}, - {14, 8}, {12, 6}, {10, 4}, { 8, 6}, { 8,10}, { 8,14}, { 8,18}}; - int threesOuterExpectedPoints[][2] = {{18, 0}, {22, 0}, {26, 0}, {28, 2}, {28, 6}, {28,10}, {28,14}, {26,16}, - {24,14}, {24,10}, {24, 6}, {22, 4}, {18, 4}, {16, 2}}; + int onesOuterExpectedPoints[][2] = {{28,16}, {36,16}, {40,20}, {40,28}, {36,32}, {28,32}, {24,28}, {24,20}}; + int twosOuterExpectedPoints[][2] = {{12, 0}, {20, 0}, {28, 0}, {32, 4}, {36, 8}, {44, 8}, + {48,12}, {48,20}, {48,28}, {48,36}, {48,44}, + {44,48}, {36,48}, {28,48}, {20,48}, {12,48}, + { 8,44}, { 8,36}, { 8,28}, { 8,20}, { 8,12}, { 8, 4}}; + int twosInnerExpectedPoints[][2] = {{20,40}, {28,40}, {36,40}, {40,36}, {40,28}, {40,20}, {36,16}, + {28,16}, {24,12}, {20, 8}, {16,12}, {16,20}, {16,28}, {16,36}}; + int threesOuterExpectedPoints[][2] = {{36, 0}, {44, 0}, {52, 0}, {56, 4}, {56,12}, {56,20}, {56,28}, {52,32}, + {48,28}, {48,20}, {48,12}, {44, 8}, {36, 8}, {32, 4}}; TestBoundaryPointsEqual(onesOuter->points, onesOuterExpectedPoints); TestBoundaryPointsEqual(twosOuter->points, twosOuterExpectedPoints); @@ -281,7 +281,7 @@ private: // version of 'points', so that the starting position doesn't need to match exactly. for (size_t i = 0; i < points.size(); i++) { - // the input numbers in expectedPoints are defined under the assumption that CELL_SIZE is 4, so let's include + // the input numbers in expectedPoints are defined under the assumption that CELL_SIZE is 2, so let's include // a scaling factor to protect against that should CELL_SIZE ever change TS_ASSERT_DELTA(points[i].X, float(expectedPoints[i][0]) * 4.f / TERRAIN_TILE_SIZE, 1e-7); TS_ASSERT_DELTA(points[i].Y, float(expectedPoints[i][1]) * 4.f / TERRAIN_TILE_SIZE, 1e-7); diff --git a/source/simulation2/helpers/Geometry.cpp b/source/simulation2/helpers/Geometry.cpp index 5b713f03da..392c34bda7 100644 --- a/source/simulation2/helpers/Geometry.cpp +++ b/source/simulation2/helpers/Geometry.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2012 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -50,7 +50,7 @@ float Geometry::ChordToCentralAngle(const float chordLength, const float radius) return acosf(1.f - SQR(chordLength)/(2.f*SQR(radius))); // cfr. law of cosines } -fixed Geometry::DistanceToSquare(CFixedVector2D point, CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize) +fixed Geometry::DistanceToSquare(CFixedVector2D point, CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize, bool countInsideAsZero) { /* * Relative to its own coordinate system, we have a square like: @@ -92,7 +92,7 @@ fixed Geometry::DistanceToSquare(CFixedVector2D point, CFixedVector2D u, CFixedV fixed closest = (dv.Absolute() - hh).Absolute(); // horizontal edges if (-hh < dv && dv < hh) // region I - closest = std::min(closest, (du.Absolute() - hw).Absolute()); // vertical edges + closest = countInsideAsZero ? fixed::Zero() : std::min(closest, (du.Absolute() - hw).Absolute()); // vertical edges return closest; } diff --git a/source/simulation2/helpers/Geometry.h b/source/simulation2/helpers/Geometry.h index e8af00ea21..33c7930c9a 100644 --- a/source/simulation2/helpers/Geometry.h +++ b/source/simulation2/helpers/Geometry.h @@ -32,51 +32,67 @@ namespace Geometry { /** - * Checks if a point is inside the given rotated square or rectangle. + * Checks if a point is inside the given rotated rectangle. + * Points precisely on an edge are considered to be inside. * - * @note Currently assumes the @p u and @p v vectors are perpendicular. - * @param point point vector of the point that is to be tested relative to the origin (center) of the shape. - * @param u rotated X axis unit vector relative to the absolute XZ plane. Indicates the orientation of the rectangle. If not rotated, - * this value is the absolute X axis unit vector (1,0). If rotated by angle theta, this should be (cos theta, -sin theta), as - * the absolute Z axis points down in the unit circle. - * @param v rotated Z axis unit vector relative to the absolute XZ plane. Indicates the orientation of the rectangle. If not rotated, - * this value is the absolute Z axis unit vector (0,1). If rotated by angle theta, this should be (sin theta, cos theta), as - * the absolute Z axis points down in the unit circle. - * @param halfSize Holds half the dimensions of the shape along the u and v vectors, respectively. + * The rectangle is defined by the four vertexes + * (+/-u*halfSize.X +/-v*halfSize.Y). * - * @return true if @p point is inside the square with rotated X axis unit vector @p u and rotated Z axis unit vector @p v, - * and half dimensions specified by @p halfSizes. + * The @p u and @p v vectors must be perpendicular. */ -bool PointIsInSquare(CFixedVector2D point, CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize); - -CFixedVector2D GetHalfBoundingBox(CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize); - -fixed DistanceToSquare(CFixedVector2D point, CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize); +bool PointIsInSquare(CFixedVector2D point, + CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize); /** - * Given a circle of radius @p radius, and a chord of length @p chordLength on this circle, computes the central angle formed by + * Returns a vector (bx,by) such that every point inside + * the given rotated rectangle has coordinates + * (x,y) with -bx <= x <= bx, -by <= y < by. + * + * The rectangle is defined by the four vertexes + * (+/-u*halfSize.X +/-v*halfSize.Y). + */ +CFixedVector2D GetHalfBoundingBox(CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize); + +/** + * Returns the minimum Euclidean distance from the given point to + * any point on the boundary of the given rotated rectangle. + * + * If @p countInsideAsZero is true, and the point is inside the rectangle, + * it will return 0. + * If @p countInsideAsZero is false, the (positive) distance to the boundary + * will be returned regardless of where the point is. + * + * The rectangle is defined by the four vertexes + * (+/-u*halfSize.X +/-v*halfSize.Y). + * + * The @p u and @p v vectors must be perpendicular and unit length. + */ +fixed DistanceToSquare(CFixedVector2D point, + CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize, + bool countInsideAsZero = false); + +/** + * Returns a point on the boundary of the given rotated rectangle + * that is closest (or equally closest) to the given point + * in Euclidean distance. + * + * The rectangle is defined by the four vertexes + * (+/-u*halfSize.X +/-v*halfSize.Y). + * + * The @p u and @p v vectors must be perpendicular and unit length. + */ +CFixedVector2D NearestPointOnSquare(CFixedVector2D point, + CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize); + +/** + * Given a circle of radius @p radius, and a chord of length @p chordLength + * on this circle, computes the central angle formed by * connecting the chord's endpoints to the center of the circle. - * + * * @param radius Radius of the circle; must be strictly positive. */ float ChordToCentralAngle(const float chordLength, const float radius); -/** - * Find point closest to the given point on the edge of the given square or rectangle. - * - * @note Currently assumes the @p u and @p v vectors are perpendicular. - * @param point point vector of the point we want to get the nearest edge point for, relative to the origin (center) of the shape. - * @param u rotated X axis unit vector, relative to the absolute XZ plane. Indicates the orientation of the shape. If not rotated, - * this value is the absolute X axis unit vector (1,0). If rotated by angle theta, this should be (cos theta, -sin theta). - * @param v rotated Z axis unit vector, relative to the absolute XZ plane. Indicates the orientation of the shape. If not rotated, - * this value is the absolute Z axis unit vector (0,1). If rotated by angle theta, this should be (sin theta, cos theta). - * @param halfSize Holds half the dimensions of the shape along the u and v vectors, respectively. - * - * @return point that is closest to @p point on the edge of the square specified by orientation unit vectors @p u and @p v and half - * dimensions @p halfSize, relative to the center of the square - */ -CFixedVector2D NearestPointOnSquare(CFixedVector2D point, CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize); - bool TestRaySquare(CFixedVector2D a, CFixedVector2D b, CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize); bool TestRayAASquare(CFixedVector2D a, CFixedVector2D b, CFixedVector2D halfSize); diff --git a/source/simulation2/helpers/HierarchicalPathfinder.cpp b/source/simulation2/helpers/HierarchicalPathfinder.cpp new file mode 100644 index 0000000000..cb9f53365c --- /dev/null +++ b/source/simulation2/helpers/HierarchicalPathfinder.cpp @@ -0,0 +1,622 @@ +/* Copyright (C) 2015 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 "HierarchicalPathfinder.h" + +#include "graphics/Overlay.h" +#include "ps/Profile.h" + +// Find the root ID of a region, used by InitRegions +inline u16 RootID(u16 x, const std::vector& v) +{ + // Just add a basic check for infinite loops + int checkLoop = 0; + while (v[x] < x) + { + ++checkLoop; + ENSURE(checkLoop < 1000 && "Long loop (probably infinite), unable to find an invariant point"); + x = v[x]; + } + + return x; +} + +void HierarchicalPathfinder::Chunk::InitRegions(int ci, int cj, Grid* grid, pass_class_t passClass) +{ + ENSURE(ci < 256 && cj < 256); // avoid overflows + m_ChunkI = ci; + m_ChunkJ = cj; + + memset(m_Regions, 0, sizeof(m_Regions)); + + int i0 = ci * CHUNK_SIZE; + int j0 = cj * CHUNK_SIZE; + int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W); + int j1 = std::min(j0 + CHUNK_SIZE, (int)grid->m_H); + + // Efficiently flood-fill the m_Regions grid + + int regionID = 0; + std::vector connect; + + u16* pCurrentID = NULL; + u16 LeftID = 0; + u16 DownID = 0; + + connect.reserve(32); // TODO: What's a sensible number? + connect.push_back(0); // connect[0] = 0 + + // Start by filling the grid with 0 for blocked, + // and regionID for unblocked + for (int j = j0; j < j1; ++j) + { + for (int i = i0; i < i1; ++i) + { + pCurrentID = &m_Regions[j-j0][i-i0]; + if (!IS_PASSABLE(grid->get(i, j), passClass)) + { + *pCurrentID = 0; + continue; + } + + if (j > j0) + DownID = m_Regions[j-1-j0][i-i0]; + + if (i == i0) + LeftID = 0; + else + LeftID = m_Regions[j-j0][i-1-i0]; + + if (LeftID > 0) + { + *pCurrentID = LeftID; + if (*pCurrentID != DownID && DownID > 0) + { + u16 id0 = RootID(DownID, connect); + u16 id1 = RootID(LeftID, connect); + + if (id0 < id1) + connect[id1] = id0; + else if (id0 > id1) + connect[id0] = id1; + } + } + else if (DownID > 0) + *pCurrentID = DownID; + else + { + // New ID + *pCurrentID = ++regionID; + connect.push_back(regionID); + } + } + } + + // Directly point the root ID + m_NumRegions = 0; + for (u16 i = regionID; i > 0; --i) + { + if (connect[i] == i) + ++m_NumRegions; + else + connect[i] = RootID(i,connect); + } + + // Replace IDs by the root ID + for (int i = 0; i < CHUNK_SIZE; ++i) + for (int j = 0; j < CHUNK_SIZE; ++j) + m_Regions[i][j] = connect[m_Regions[i][j]]; +} + +/** + * Returns a RegionID for the given global navcell coords + * (which must be inside this chunk); + */ +HierarchicalPathfinder::RegionID HierarchicalPathfinder::Chunk::Get(int i, int j) const +{ + ENSURE(i < CHUNK_SIZE && j < CHUNK_SIZE); + return RegionID(m_ChunkI, m_ChunkJ, m_Regions[j][i]); +} + +/** + * Return the global navcell coords that correspond roughly to the + * center of the given region in this chunk. + * (This is not guaranteed to be actually inside the region.) + */ +void HierarchicalPathfinder::Chunk::RegionCenter(u16 r, int& i_out, int& j_out) const +{ + // Find the mean of i,j coords of navcells in this region: + + u32 si = 0, sj = 0; // sum of i,j coords + u32 n = 0; // number of navcells in region + + cassert(CHUNK_SIZE < 256); // conservative limit to ensure si and sj don't overflow + + for (int j = 0; j < CHUNK_SIZE; ++j) + { + for (int i = 0; i < CHUNK_SIZE; ++i) + { + if (m_Regions[j][i] == r) + { + si += i; + sj += j; + n += 1; + } + } + } + + // Avoid divide-by-zero + if (n == 0) + n = 1; + + i_out = m_ChunkI * CHUNK_SIZE + si / n; + j_out = m_ChunkJ * CHUNK_SIZE + sj / n; +} + +/** + * Returns whether any navcell in the given region is inside the goal. + */ +bool HierarchicalPathfinder::Chunk::RegionContainsGoal(u16 r, const PathGoal& goal) const +{ + // Inefficiently check every single navcell: + for (u16 j = 0; j < CHUNK_SIZE; ++j) + { + for (u16 i = 0; i < CHUNK_SIZE; ++i) + { + if (m_Regions[j][i] == r) + { + if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j)) + return true; + } + } + } + + return false; +} + +/** + * Returns the global navcell coords, and the squared distance to the goal + * navcell, of whichever navcell inside the given region is closest to + * that goal. + */ +void HierarchicalPathfinder::Chunk::RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const +{ + iBest = 0; + jBest = 0; + dist2Best = std::numeric_limits::max(); + + for (int j = 0; j < CHUNK_SIZE; ++j) + { + for (int i = 0; i < CHUNK_SIZE; ++i) + { + if (m_Regions[j][i] == r) + { + u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - iGoal)*(i + m_ChunkI*CHUNK_SIZE - iGoal) + + (j + m_ChunkJ*CHUNK_SIZE - jGoal)*(j + m_ChunkJ*CHUNK_SIZE - jGoal); + + if (dist2 < dist2Best) + { + iBest = i + m_ChunkI*CHUNK_SIZE; + jBest = j + m_ChunkJ*CHUNK_SIZE; + dist2Best = dist2; + } + } + } + } +} + +HierarchicalPathfinder::HierarchicalPathfinder() : m_DebugOverlay(NULL) +{ +} + +HierarchicalPathfinder::~HierarchicalPathfinder() +{ + SAFE_DELETE(m_DebugOverlay); +} + +void HierarchicalPathfinder::SetDebugOverlay(bool enabled, const CSimContext* simContext) +{ + if (enabled && !m_DebugOverlay) + { + m_DebugOverlay = new HierarchicalOverlay(*this); + m_DebugOverlayLines.clear(); + m_SimContext = simContext; + AddDebugEdges(GetPassabilityClass("default")); + } + else if (!enabled && m_DebugOverlay) + { + SAFE_DELETE(m_DebugOverlay); + m_DebugOverlayLines.clear(); + m_SimContext = NULL; + } +} + +void HierarchicalPathfinder::Recompute(const std::map& passClassMasks, Grid* grid) +{ + PROFILE3("Hierarchical Recompute"); + + m_PassClassMasks = passClassMasks; + + m_W = grid->m_W; + m_H = grid->m_H; + + // Divide grid into chunks with round-to-positive-infinity + m_ChunksW = (grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE; + m_ChunksH = (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE; + + ENSURE(m_ChunksW < 256 && m_ChunksH < 256); // else the u8 Chunk::m_ChunkI will overflow + + m_Chunks.clear(); + m_Edges.clear(); + + for (auto& passClassMask : passClassMasks) + { + pass_class_t passClass = passClassMask.second; + + // Compute the regions within each chunk + m_Chunks[passClass].resize(m_ChunksW*m_ChunksH); + for (int cj = 0; cj < m_ChunksH; ++cj) + { + for (int ci = 0; ci < m_ChunksW; ++ci) + { + m_Chunks[passClass].at(cj*m_ChunksW + ci).InitRegions(ci, cj, grid, passClass); + } + } + + // Construct the search graph over the regions + + EdgesMap& edges = m_Edges[passClass]; + + for (int cj = 0; cj < m_ChunksH; ++cj) + { + for (int ci = 0; ci < m_ChunksW; ++ci) + { + FindEdges(ci, cj, passClass, edges); + } + } + } + + if (m_DebugOverlay) + { + PROFILE("debug overlay"); + m_DebugOverlayLines.clear(); + AddDebugEdges(GetPassabilityClass("default")); + } +} + +void HierarchicalPathfinder::Update(Grid* grid, const Grid* dirtinessGrid) +{ + PROFILE3("Hierarchical Update"); + + std::vector > processedChunks; + for (int j = 0; j < dirtinessGrid->m_H; ++j) + { + for (int i = 0; i < dirtinessGrid->m_W; ++i) + { + if (!dirtinessGrid->get(i, j)) + continue; + + std::pair chunkID(i / CHUNK_SIZE, j / CHUNK_SIZE); + + for (auto& passClassMask : m_PassClassMasks) + { + pass_class_t passClass = passClassMask.second; + Chunk& a = m_Chunks[passClass].at(chunkID.second*m_ChunksW + chunkID.first); + if (std::find(processedChunks.begin(), processedChunks.end(), chunkID) == processedChunks.end()) + { + processedChunks.push_back(chunkID); + a.InitRegions(chunkID.first, chunkID.second, grid, passClass); + } + } + } + } + + // TODO: Also be clever with edges + m_Edges.clear(); + for (auto& passClassMask : m_PassClassMasks) + { + pass_class_t passClass = passClassMask.second; + EdgesMap& edges = m_Edges[passClass]; + + for (int cj = 0; cj < m_ChunksH; ++cj) + { + for (int ci = 0; ci < m_ChunksW; ++ci) + { + FindEdges(ci, cj, passClass, edges); + } + } + } + + if (m_DebugOverlay) + { + PROFILE("debug overlay"); + m_DebugOverlayLines.clear(); + AddDebugEdges(GetPassabilityClass("default")); + } +} + +/** + * Find edges between regions in this chunk and the adjacent below/left chunks. + */ +void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) +{ + std::vector& chunks = m_Chunks[passClass]; + + Chunk& a = chunks.at(cj*m_ChunksW + ci); + + // For each edge between chunks, we loop over every adjacent pair of + // navcells in the two chunks. If they are both in valid regions + // (i.e. are passable navcells) then add a graph edge between those regions. + // (We don't need to test for duplicates since EdgesMap already uses a + // std::set which will drop duplicate entries.) + + if (ci > 0) + { + Chunk& b = chunks.at(cj*m_ChunksW + (ci-1)); + for (int j = 0; j < CHUNK_SIZE; ++j) + { + RegionID ra = a.Get(0, j); + RegionID rb = b.Get(CHUNK_SIZE-1, j); + if (ra.r && rb.r) + { + edges[ra].insert(rb); + edges[rb].insert(ra); + } + } + } + + if (cj > 0) + { + Chunk& b = chunks.at((cj-1)*m_ChunksW + ci); + for (int i = 0; i < CHUNK_SIZE; ++i) + { + RegionID ra = a.Get(i, 0); + RegionID rb = b.Get(i, CHUNK_SIZE-1); + if (ra.r && rb.r) + { + edges[ra].insert(rb); + edges[rb].insert(ra); + } + } + } + +} + +/** + * Debug visualisation of graph edges between regions. + */ +void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass) +{ + const EdgesMap& edges = m_Edges[passClass]; + const std::vector& chunks = m_Chunks[passClass]; + + for (auto& edge : edges) + { + for (const RegionID& region: edge.second) + { + // Draw a line between the two regions' centers + + int i0, j0, i1, j1; + chunks[edge.first.cj * m_ChunksW + edge.first.ci].RegionCenter(edge.first.r, i0, j0); + chunks[region.cj * m_ChunksW + region.ci].RegionCenter(region.r, i1, j1); + + CFixedVector2D a, b; + Pathfinding::NavcellCenter(i0, j0, a.X, a.Y); + Pathfinding::NavcellCenter(i1, j1, b.X, b.Y); + + // Push the endpoints inwards a little to avoid overlaps + CFixedVector2D d = b - a; + d.Normalize(entity_pos_t::FromInt(1)); + a += d; + b -= d; + + std::vector xz; + xz.push_back(a.X.ToFloat()); + xz.push_back(a.Y.ToFloat()); + xz.push_back(b.X.ToFloat()); + xz.push_back(b.Y.ToFloat()); + + m_DebugOverlayLines.emplace_back(); + m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0); + SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true); + } + } +} + +HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) +{ + int ci = i / CHUNK_SIZE; + int cj = j / CHUNK_SIZE; + ENSURE(ci < m_ChunksW && cj < m_ChunksH); + return m_Chunks[passClass][cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE); +} + +bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) +{ + RegionID source = Get(i0, j0, passClass); + + // Find everywhere that's reachable + std::set reachableRegions; + FindReachableRegions(source, reachableRegions, passClass); + + // Check whether any reachable region contains the goal + for (const RegionID& region : reachableRegions) + { + // Skip region if its chunk doesn't contain the goal area + entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (region.ci * CHUNK_SIZE); + entity_pos_t z0 = Pathfinding::NAVCELL_SIZE * (region.cj * CHUNK_SIZE); + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE; + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE; + if (!goal.RectContainsGoal(x0, z0, x1, z1)) + continue; + + // If the region contains the goal area, the goal is reachable + // and we don't need to move it + if (GetChunk(region.ci, region.cj, passClass).RegionContainsGoal(region.r, goal)) + return false; + } + + // The goal area wasn't reachable, + // so find the navcell that's nearest to the goal's center + + u16 iGoal, jGoal; + Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H); + + FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass); + + // Construct a new point goal at the nearest reachable navcell + PathGoal newGoal; + newGoal.type = PathGoal::POINT; + Pathfinding::NavcellCenter(iGoal, jGoal, newGoal.x, newGoal.z); + goal = newGoal; + + return true; +} + +void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) +{ + std::set regions; + FindPassableRegions(regions, passClass); + FindNearestNavcellInRegions(regions, i, j, passClass); +} + +void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) +{ + // Find the navcell in the given regions that's nearest to the goal navcell: + // * For each region, record the (squared) minimal distance to the goal point + // * Sort regions by that underestimated distance + // * For each region, find the actual nearest navcell + // * Stop when the underestimated distances are worse than the best real distance + + std::vector > regionDistEsts; // pair of (distance^2, region) + + for (const RegionID& region : regions) + { + int i0 = region.ci * CHUNK_SIZE; + int j0 = region.cj * CHUNK_SIZE; + int i1 = i0 + CHUNK_SIZE - 1; + int j1 = j0 + CHUNK_SIZE - 1; + + // Pick the point in the chunk nearest the goal + int iNear = Clamp((int)iGoal, i0, i1); + int jNear = Clamp((int)jGoal, j0, j1); + + int dist2 = (iNear - iGoal)*(iNear - iGoal) + + (jNear - jGoal)*(jNear - jGoal); + + regionDistEsts.emplace_back(dist2, region); + } + + // Sort by increasing distance (tie-break on RegionID) + std::sort(regionDistEsts.begin(), regionDistEsts.end()); + + int iBest = iGoal; + int jBest = jGoal; + u32 dist2Best = std::numeric_limits::max(); + + for (auto& pair : regionDistEsts) + { + if (pair.first >= dist2Best) + break; + + RegionID region = pair.second; + + int i, j; + u32 dist2; + GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, i, j, dist2); + + if (dist2 < dist2Best) + { + iBest = i; + jBest = j; + dist2Best = dist2; + } + } + + iGoal = iBest; + jGoal = jBest; +} + +void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) +{ + // Flood-fill the region graph, starting at 'from', + // collecting all the regions that are reachable via edges + + std::vector open; + open.push_back(from); + reachable.insert(from); + + while (!open.empty()) + { + RegionID curr = open.back(); + open.pop_back(); + + for (const RegionID& region : m_Edges[passClass][curr]) + // Add to the reachable set; if this is the first time we added + // it then also add it to the open list + if (reachable.insert(region).second) + open.push_back(region); + } +} + +void HierarchicalPathfinder::FindPassableRegions(std::set& regions, pass_class_t passClass) +{ + // Construct a set of all regions of all chunks for this pass class + for (const Chunk& chunk : m_Chunks[passClass]) + { + // region 0 is impassable tiles + for (int r = 1; r <= chunk.m_NumRegions; ++r) + regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r)); + } +} + +Grid HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) +{ + Grid connectivityGrid(m_W, m_H); + connectivityGrid.reset(); + + u16 idx = 1; + + for (size_t j1 = 0; j1 < m_H; ++j1) + { + for (size_t i1 = 0; i1 < m_W; ++i1) + { + if (connectivityGrid.get(i1, j1) != 0) + continue; + + RegionID region = Get(i1, j1, passClass); + if (region.r == 0) + continue; + + std::set reachable; + FindReachableRegions(region, reachable, passClass); + for (size_t j2 = 0; j2 < m_H; ++j2) + { + for (size_t i2 = 0; i2 < m_W; ++i2) + { + if (std::find(reachable.begin(), reachable.end(), Get(i2, j2, passClass)) != reachable.end()) + connectivityGrid.set(i1, j1, idx); + } + } + ++idx; + } + } + + return connectivityGrid; +} diff --git a/source/simulation2/helpers/HierarchicalPathfinder.h b/source/simulation2/helpers/HierarchicalPathfinder.h new file mode 100644 index 0000000000..9bed1ce64e --- /dev/null +++ b/source/simulation2/helpers/HierarchicalPathfinder.h @@ -0,0 +1,215 @@ +/* Copyright (C) 2015 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 . + */ + +#ifndef INCLUDED_HIERPATHFINDER +#define INCLUDED_HIERPATHFINDER + +#include "Pathfinding.h" + +#include "renderer/TerrainOverlay.h" +#include "Render.h" +#include "graphics/SColor.h" + +/** + * Hierarchical pathfinder. + * + * It doesn't find shortest paths, but deals with connectivity. + * + * The navcell-grid representation of the map is split into fixed-size chunks. + * Within a chunk, each maximal set of adjacently-connected passable navcells + * is defined as a region. + * Each region is a vertex in the hierarchical pathfinder's graph. + * When two regions in adjacent chunks are connected by passable navcells, + * the graph contains an edge between the corresponding two vertexes. + * (There will never be an edge between two regions in the same chunk.) + * + * Since regions are typically fairly large, it is possible to determine + * connectivity between any two navcells by mapping them onto their appropriate + * region and then doing a relatively small graph search. + */ + +class HierarchicalOverlay; + +class HierarchicalPathfinder +{ +public: + struct RegionID + { + u8 ci, cj; // chunk ID + u16 r; // unique-per-chunk local region ID + + RegionID(u8 ci, u8 cj, u16 r) : ci(ci), cj(cj), r(r) { } + + bool operator<(RegionID b) const + { + // Sort by chunk ID, then by per-chunk region ID + if (ci < b.ci) + return true; + if (b.ci < ci) + return false; + if (cj < b.cj) + return true; + if (b.cj < cj) + return false; + return r < b.r; + } + + bool operator==(RegionID b) const + { + return ((ci == b.ci) && (cj == b.cj) && (r == b.r)); + } + }; + + HierarchicalPathfinder(); + ~HierarchicalPathfinder(); + + void SetDebugOverlay(bool enabled, const CSimContext* simContext); + + void Recompute(const std::map& passClassMasks, Grid* passabilityGrid); + void Update(Grid* grid, const Grid* dirtinessGrid); + + RegionID Get(u16 i, u16 j, pass_class_t passClass); + + /** + * Updates @p goal so that it's guaranteed to be reachable from the navcell + * @p i0, @p j0 (which is assumed to be on a passable navcell). + * If any part of the goal area is already reachable then + * nothing is changed; otherwise the goal is replaced with a point goal + * at the nearest reachable navcell to the original goal's center. + * Returns true if the goal was replaced. + */ + bool MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass); + + /** + * Updates @p i, @p j (which is assumed to be an impassable navcell) + * to the nearest passable navcell. + */ + void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass); + + /** + * Generates the connectivity grid associated with the given pass_class + */ + Grid GetConnectivityGrid(pass_class_t passClass); + + pass_class_t GetPassabilityClass(const std::string& name) const + { + auto it = m_PassClassMasks.find(name); + if (it != m_PassClassMasks.end()) + return it->second; + + LOGERROR("Invalid passability class name '%s'", name.c_str()); + return 0; + } + +private: + static const u8 CHUNK_SIZE = 96; // number of navcells per side + // TODO PATHFINDER: figure out best number. Probably 64 < n < 128 + + struct Chunk + { + u8 m_ChunkI, m_ChunkJ; // chunk ID + u16 m_NumRegions; // number of local region IDs (starting from 1) + u16 m_Regions[CHUNK_SIZE][CHUNK_SIZE]; // local region ID per navcell + + cassert(CHUNK_SIZE*CHUNK_SIZE/2 < 65536); // otherwise we could overflow m_NumRegions with a checkerboard pattern + + void InitRegions(int ci, int cj, Grid* grid, pass_class_t passClass); + + RegionID Get(int i, int j) const; + + void RegionCenter(u16 r, int& i, int& j) const; + + bool RegionContainsGoal(u16 r, const PathGoal& goal) const; + + void RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const; + }; + + typedef std::map > EdgesMap; + + void FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges); + + void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass); + + void FindPassableRegions(std::set& regions, pass_class_t passClass); + + /** + * Updates @p iGoal and @p jGoal to the navcell that is the nearest to the + * initial goal coordinates, in one of the given @p regions. + * (Assumes @p regions is non-empty.) + */ + void FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass); + + Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) + { + return m_Chunks[passClass].at(cj * m_ChunksW + ci); + } + + u16 m_W, m_H; + u16 m_ChunksW, m_ChunksH; + std::map > m_Chunks; + + std::map m_Edges; + + std::map m_PassClassMasks; + + void AddDebugEdges(pass_class_t passClass); + HierarchicalOverlay* m_DebugOverlay; + const CSimContext* m_SimContext; // Used for drawing the debug lines + +public: + std::vector m_DebugOverlayLines; +}; + +class HierarchicalOverlay : public TerrainTextureOverlay +{ +public: + HierarchicalPathfinder& m_PathfinderHier; + + HierarchicalOverlay(HierarchicalPathfinder& pathfinderHier) : + TerrainTextureOverlay(Pathfinding::NAVCELLS_PER_TILE), m_PathfinderHier(pathfinderHier) + { + } + + virtual void BuildTextureRGBA(u8* data, size_t w, size_t h) + { + pass_class_t passClass = m_PathfinderHier.GetPassabilityClass("default"); + + for (size_t j = 0; j < h; ++j) + { + for (size_t i = 0; i < w; ++i) + { + SColor4ub color; + + HierarchicalPathfinder::RegionID rid = m_PathfinderHier.Get(i, j, passClass); + if (rid.r == 0) + color = SColor4ub(0, 0, 0, 0); + else if (rid.r == 0xFFFF) + color = SColor4ub(255, 0, 255, 255); + else + color = GetColor(rid.r + rid.ci*5 + rid.cj*7, 127); + + *data++ = color.R; + *data++ = color.G; + *data++ = color.B; + *data++ = color.A; + } + } + } +}; + + +#endif // INCLUDED_HIERPATHFINDER diff --git a/source/simulation2/helpers/LongPathfinder.cpp b/source/simulation2/helpers/LongPathfinder.cpp new file mode 100644 index 0000000000..e79186b674 --- /dev/null +++ b/source/simulation2/helpers/LongPathfinder.cpp @@ -0,0 +1,1223 @@ +/* Copyright (C) 2015 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 "LongPathfinder.h" + +#include "lib/bits.h" +#include "ps/Profile.h" + +/** + * Jump point cache. + * + * The JPS algorithm wants to efficiently either find the first jump point + * in some direction from some cell (not counting the cell itself), + * if it is reachable without crossing any impassable cells; + * or know that there is no such reachable jump point. + * The jump point is always on a passable cell. + * We cache that data to allow fast lookups, which helps performance + * significantly (especially on sparse maps). + * Recalculation might be expensive but the underlying passability data + * changes relatively rarely. + * + * To allow the algorithm to detect goal cells, we want to treat them as + * jump points too. (That means the algorithm will push those cells onto + * its open queue, and will eventually pop a goal cell and realise it's done.) + * (Goals might be circles/squares/etc, not just a single cell.) + * But the goal generally changes for every path request, so we can't cache + * it like the normal jump points. + * Instead, if there's no jump point from some cell then we'll cache the + * first impassable cell as an 'obstruction jump point' + * (with a flag to distinguish from a real jump point), and then the caller + * can test whether the goal includes a cell that's closer than the first + * (obstruction or real) jump point, + * and treat the goal cell as a jump point in that case. + * + * We only ever need to find the jump point relative to a passable cell; + * the cache is allowed to return bogus values for impassable cells. + */ +class JumpPointCache +{ + /** + * Simple space-inefficient row storage. + */ + struct RowRaw + { + std::vector data; + + size_t GetMemoryUsage() const + { + return data.capacity() * sizeof(u16); + } + + RowRaw(int length) + { + data.resize(length); + } + + /** + * Set cells x0 <= x < x1 to have jump point x1. + */ + void SetRange(int x0, int x1, bool obstruction) + { + ENSURE(0 <= x0 && x0 <= x1 && x1 < (int)data.size()); + for (int x = x0; x < x1; ++x) + data[x] = (x1 << 1) | (obstruction ? 1 : 0); + } + + /** + * Returns the coordinate of the next jump point xp (where x < xp), + * and whether it's an obstruction point or jump point. + */ + void Get(int x, int& xp, bool& obstruction) + { + ENSURE(0 <= x && x < (int)data.size()); + xp = data[x] >> 1; + obstruction = data[x] & 1; + } + + void Finish() { } + }; + + struct RowTree + { + /** + * Represents an interval [u15 x0, u16 x1) + * with a boolean obstruction flag, + * packed into a single u32. + */ + struct Interval + { + Interval() : data(0) { } + + Interval(int x0, int x1, bool obstruction) + { + ENSURE(0 <= x0 && x0 < 0x8000); + ENSURE(0 <= x1 && x1 < 0x10000); + data = ((u32)x0 << 17) | (u32)(obstruction ? 0x10000 : 0) | (u32)x1; + } + + int x0() { return data >> 17; } + int x1() { return data & 0xFFFF; } + bool obstruction() { return (data & 0x10000) != 0; } + + u32 data; + }; + + std::vector data; + + size_t GetMemoryUsage() const + { + return data.capacity() * sizeof(Interval); + } + + RowTree(int UNUSED(length)) + { + } + + void SetRange(int x0, int x1, bool obstruction) + { + ENSURE(0 <= x0 && x0 <= x1); + data.emplace_back(x0, x1, obstruction); + } + + /** + * Recursive helper function for Finish(). + * Given two ranges [x0, pivot) and [pivot, x1) in the sorted array 'data', + * the pivot element is added onto the binary tree (stored flattened in an + * array), and then each range is split into two sub-ranges with a pivot in + * the middle (to ensure the tree remains balanced) and ConstructTree recurses. + */ + void ConstructTree(std::vector& tree, size_t x0, size_t pivot, size_t x1, size_t idx_tree) + { + ENSURE(x0 < data.size()); + ENSURE(x1 <= data.size()); + ENSURE(x0 <= pivot); + ENSURE(pivot < x1); + ENSURE(idx_tree < tree.size()); + + tree[idx_tree] = data[pivot]; + + if (x0 < pivot) + ConstructTree(tree, x0, (x0 + pivot) / 2, pivot, (idx_tree << 1) + 1); + if (pivot + 1 < x1) + ConstructTree(tree, pivot + 1, (pivot + x1) / 2, x1, (idx_tree << 1) + 2); + } + + void Finish() + { + // Convert the sorted interval list into a balanced binary tree + + std::vector tree; + + if (!data.empty()) + { + size_t depth = ceil_log2(data.size() + 1); + tree.resize((1 << depth) - 1); + ConstructTree(tree, 0, data.size() / 2, data.size(), 0); + } + + data.swap(tree); + } + + void Get(int x, int& xp, bool& obstruction) + { + // Search the binary tree for an interval which contains x + int i = 0; + while (true) + { + ENSURE(i < (int)data.size()); + Interval interval = data[i]; + if (x < interval.x0()) + i = (i << 1) + 1; + else if (x >= interval.x1()) + i = (i << 1) + 2; + else + { + ENSURE(interval.x0() <= x && x < interval.x1()); + xp = interval.x1(); + obstruction = interval.obstruction(); + return; + } + } + } + }; + + // Pick one of the row implementations + typedef RowRaw Row; + +public: + int m_Width; + int m_Height; + std::vector m_JumpPointsRight; + std::vector m_JumpPointsLeft; + std::vector m_JumpPointsUp; + std::vector m_JumpPointsDown; + + /** + * Compute the cached obstruction/jump points for each cell, + * in a single direction. By default the code assumes the rightwards + * (+i) direction; set 'transpose' to switch to upwards (+j), + * and/or set 'mirror' to reverse the direction. + */ + void ComputeRows(std::vector& rows, + const Grid& terrain, pass_class_t passClass, + bool transpose, bool mirror) + { + int w = terrain.m_W; + int h = terrain.m_H; + + if (transpose) + std::swap(w, h); + + // Check the terrain passability, adjusted for transpose/mirror +#define TERRAIN_IS_PASSABLE(i, j) \ + IS_PASSABLE( \ + mirror \ + ? (transpose ? terrain.get((j), w-1-(i)) : terrain.get(w-1-(i), (j))) \ + : (transpose ? terrain.get((j), (i)) : terrain.get((i), (j))) \ + , passClass) + + rows.reserve(h); + for (int j = 0; j < h; ++j) + rows.emplace_back(w); + + for (int j = 1; j < h - 1; ++j) + { + // Find the first passable cell. + // Then, find the next jump/obstruction point after that cell, + // and store that point for the passable range up to that cell, + // then repeat. + + int i = 0; + while (i < w) + { + // Restart the 'while' loop until we reach a passable cell + if (!TERRAIN_IS_PASSABLE(i, j)) + { + ++i; + continue; + } + + // i is now a passable cell; find the next jump/obstruction point. + // (We assume the map is surrounded by impassable cells, so we don't + // need to explicitly check for world bounds here.) + + int i0 = i; + while (true) + { + ++i; + + // Check if we hit an obstructed tile + if (!TERRAIN_IS_PASSABLE(i, j)) + { + rows[j].SetRange(i0, i, true); + break; + } + + // Check if we reached a jump point +#if ACCEPT_DIAGONAL_GAPS + if ((!TERRAIN_IS_PASSABLE(i, j - 1) && TERRAIN_IS_PASSABLE(i + 1, j - 1)) || + (!TERRAIN_IS_PASSABLE(i, j + 1) && TERRAIN_IS_PASSABLE(i + 1, j + 1))) +#else + if ((!TERRAIN_IS_PASSABLE(i - 1, j - 1) && TERRAIN_IS_PASSABLE(i, j - 1)) || + (!TERRAIN_IS_PASSABLE(i - 1, j + 1) && TERRAIN_IS_PASSABLE(i, j + 1))) +#endif + { + rows[j].SetRange(i0, i, false); + break; + } + } + } + + rows[j].Finish(); + } +#undef TERRAIN_IS_PASSABLE + } + + void reset(const Grid* terrain, pass_class_t passClass) + { + PROFILE3("JumpPointCache reset"); + TIMER(L"JumpPointCache reset"); + + m_Width = terrain->m_W; + m_Height = terrain->m_H; + + ComputeRows(m_JumpPointsRight, *terrain, passClass, false, false); + ComputeRows(m_JumpPointsLeft, *terrain, passClass, false, true); + ComputeRows(m_JumpPointsUp, *terrain, passClass, true, false); + ComputeRows(m_JumpPointsDown, *terrain, passClass, true, true); + } + + size_t GetMemoryUsage() const + { + size_t bytes = 0; + for (int i = 0; i < m_Width; ++i) + { + bytes += m_JumpPointsUp[i].GetMemoryUsage(); + bytes += m_JumpPointsDown[i].GetMemoryUsage(); + } + for (int j = 0; j < m_Height; ++j) + { + bytes += m_JumpPointsRight[j].GetMemoryUsage(); + bytes += m_JumpPointsLeft[j].GetMemoryUsage(); + } + return bytes; + } + + /** + * Returns the next jump point (or goal point) to explore, + * at (ip, j) where i < ip. + * Returns i if there is no such point. + */ + int GetJumpPointRight(int i, int j, const PathGoal& goal) + { + int ip; + bool obstruction; + m_JumpPointsRight[j].Get(i, ip, obstruction); + // Adjust ip to be a goal cell, if there is one closer than the jump point; + // and then return the new ip if there is a goal, + // or the old ip if there is a (non-obstruction) jump point + if (goal.NavcellRectContainsGoal(i + 1, j, ip - 1, j, &ip, NULL) || !obstruction) + return ip; + return i; + } + + int GetJumpPointLeft(int i, int j, const PathGoal& goal) + { + int mip; // mirrored value, because m_JumpPointsLeft is generated from a mirrored map + bool obstruction; + m_JumpPointsLeft[j].Get(m_Width - 1 - i, mip, obstruction); + int ip = m_Width - 1 - mip; + if (goal.NavcellRectContainsGoal(i - 1, j, ip + 1, j, &ip, NULL) || !obstruction) + return ip; + return i; + } + + int GetJumpPointUp(int i, int j, const PathGoal& goal) + { + int jp; + bool obstruction; + m_JumpPointsUp[i].Get(j, jp, obstruction); + if (goal.NavcellRectContainsGoal(i, j + 1, i, jp - 1, NULL, &jp) || !obstruction) + return jp; + return j; + } + + int GetJumpPointDown(int i, int j, const PathGoal& goal) + { + int mjp; // mirrored value + bool obstruction; + m_JumpPointsDown[i].Get(m_Height - 1 - j, mjp, obstruction); + int jp = m_Height - 1 - mjp; + if (goal.NavcellRectContainsGoal(i, j - 1, i, jp + 1, NULL, &jp) || !obstruction) + return jp; + return j; + } +}; + +////////////////////////////////////////////////////////// + +#define PASSABLE(i, j) IS_PASSABLE(state.terrain->get(i, j), state.passClass) + +// Calculate heuristic cost from tile i,j to goal +// (This ought to be an underestimate for correctness) +PathCost LongPathfinder::CalculateHeuristic(int i, int j, int iGoal, int jGoal) +{ + int di = abs(i - iGoal); + int dj = abs(j - jGoal); + int diag = std::min(di, dj); + return PathCost(di - diag + dj - diag, diag); +} + +// Do the A* processing for a neighbour tile i,j. +void LongPathfinder::ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state) +{ + // Reject impassable tiles + if (!PASSABLE(i, j)) + return; + + PathfindTile& n = state.tiles->get(i, j); + + if (n.IsClosed()) + return; + + PathCost dg; + if (pi == i) + dg = PathCost::horizvert(abs(pj - j)); + else if (pj == j) + dg = PathCost::horizvert(abs(pi - i)); + else + { + ASSERT(abs((int)pi - (int)i) == abs((int)pj - (int)j)); // must be 45 degrees + dg = PathCost::diag(abs((int)pi - (int)i)); + } + + PathCost g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor + + PathCost h = CalculateHeuristic(i, j, state.iGoal, state.jGoal); + + // If this is a new tile, compute the heuristic distance + if (n.IsUnexplored()) + { + // Remember the best tile we've seen so far, in case we never actually reach the target + if (h < state.hBest) + { + state.hBest = h; + state.iBest = i; + state.jBest = j; + } + } + else + { + // 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 >= n.GetCost()) + return; + + // Otherwise, we have a better path. + + // If we've already added this tile to the open list: + if (n.IsOpen()) + { + // This is a better path, so replace the old one with the new cost/parent + PathCost gprev = n.GetCost(); + n.SetCost(g); + n.SetPred(pi, pj, i, j); + state.open.promote(TileID(i, j), gprev + h, g + h, h); + return; + } + } + + // Add it to the open list: + n.SetStatusOpen(); + n.SetCost(g); + n.SetPred(pi, pj, i, j); + PriorityQueue::Item t = { TileID(i, j), g + h, h }; + state.open.push(t); +} + +/* + * In the JPS algorithm, after a tile is taken off the open queue, + * we don't process every adjacent neighbour (as in standard A*). + * Instead we only move in a subset of directions (depending on the + * direction from the predecessor); and instead of moving by a single + * cell, we move up to the next jump point in that direction. + * The AddJumped... functions do this by calling ProcessNeighbour + * on the jump point (if any) in a certain direction. + * The HasJumped... functions return whether there is any jump point + * in that direction. + */ + +// JPS functions scan navcells towards one direction +// OnTheWay tests whether we are scanning towards the right direction, to avoid useless scans +inline bool OnTheWay(int i, int j, int di, int dj, const PathGoal& goal) +{ + entity_pos_t hw, hh; // half width/height of goal bounding box + CFixedVector2D hbb = Geometry::GetHalfBoundingBox(goal.u, goal.v, CFixedVector2D(goal.hw, goal.hh)); + + switch (goal.type) + { + case PathGoal::POINT: + hw = fixed::Zero(); + hh = fixed::Zero(); + break; + case PathGoal::CIRCLE: + case PathGoal::INVERTED_CIRCLE: + hw = goal.hw; + hh = goal.hw; + break; + case PathGoal::SQUARE: + case PathGoal::INVERTED_SQUARE: + hw = hbb.X.Absolute(); + hh = hbb.Y.Absolute(); + break; + NODEFAULT; + } + + if (dj != 0) + { + // Farthest goal point, z-direction + int gj = ((goal.z + (dj > 0 ? hh : -hh)) / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity(); + if ((gj - j)*dj < 0) // we're not moving towards the goal + return false; + } + else + { + if (j < ((goal.z - hh) / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() || + j >((goal.z + hh) / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity()) + return false; + } + + if (di != 0) + { + // Farthest goal point, x-direction + int gi = ((goal.x + (di > 0 ? hw : -hw)) / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity(); + if ((gi - i)*di < 0) // we're not moving towards the goal + return false; + } + else + { + if (i < ((goal.x - hw) / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() || + i >((goal.x + hh) / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity()) + return false; + } + + return true; +} + + +void LongPathfinder::AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal) +{ + if (m_UseJPSCache) + { + int jump; + if (di > 0) + jump = state.jpc->GetJumpPointRight(i, j, state.goal); + else + jump = state.jpc->GetJumpPointLeft(i, j, state.goal); + + if (jump != i) + ProcessNeighbour(i, j, jump, j, g, state); + } + else + { + ASSERT(di == 1 || di == -1); + int ni = i + di; + while (true) + { + if (!PASSABLE(ni, j)) + break; + + if (detectGoal && state.goal.NavcellContainsGoal(ni, j)) + { + state.open.clear(); + ProcessNeighbour(i, j, ni, j, g, state); + break; + } + +#if ACCEPT_DIAGONAL_GAPS + if ((!PASSABLE(ni, j - 1) && PASSABLE(ni + di, j - 1)) || + (!PASSABLE(ni, j + 1) && PASSABLE(ni + di, j + 1))) +#else + if ((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) || + (!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1))) +#endif + { + ProcessNeighbour(i, j, ni, j, g, state); + break; + } + + ni += di; + } + } +} + +// Returns the i-coordinate of the jump point if it exists, else returns i +int LongPathfinder::HasJumpedHoriz(int i, int j, int di, PathfinderState& state, bool detectGoal) +{ + if (m_UseJPSCache) + { + int jump; + if (di > 0) + jump = state.jpc->GetJumpPointRight(i, j, state.goal); + else + jump = state.jpc->GetJumpPointLeft(i, j, state.goal); + + return jump; + } + else + { + ASSERT(di == 1 || di == -1); + int ni = i + di; + while (true) + { + if (!PASSABLE(ni, j)) + return i; + + if (detectGoal && state.goal.NavcellContainsGoal(ni, j)) + { + state.open.clear(); + return ni; + } + +#if ACCEPT_DIAGONAL_GAPS + if ((!PASSABLE(ni, j - 1) && PASSABLE(ni + di, j - 1)) || + (!PASSABLE(ni, j + 1) && PASSABLE(ni + di, j + 1))) +#else + if ((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) || + (!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1))) +#endif + return ni; + + ni += di; + } + } +} + +void LongPathfinder::AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal) +{ + if (m_UseJPSCache) + { + int jump; + if (dj > 0) + jump = state.jpc->GetJumpPointUp(i, j, state.goal); + else + jump = state.jpc->GetJumpPointDown(i, j, state.goal); + + if (jump != j) + ProcessNeighbour(i, j, i, jump, g, state); + } + else + { + ASSERT(dj == 1 || dj == -1); + int nj = j + dj; + while (true) + { + if (!PASSABLE(i, nj)) + break; + + if (detectGoal && state.goal.NavcellContainsGoal(i, nj)) + { + state.open.clear(); + ProcessNeighbour(i, j, i, nj, g, state); + break; + } + +#if ACCEPT_DIAGONAL_GAPS + if ((!PASSABLE(i - 1, nj) && PASSABLE(i - 1, nj + dj)) || + (!PASSABLE(i + 1, nj) && PASSABLE(i + 1, nj + dj))) +#else + if ((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) || + (!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj))) +#endif + { + ProcessNeighbour(i, j, i, nj, g, state); + break; + } + + nj += dj; + } + } +} + +// Returns the j-coordinate of the jump point if it exists, else returns j +int LongPathfinder::HasJumpedVert(int i, int j, int dj, PathfinderState& state, bool detectGoal) +{ + if (m_UseJPSCache) + { + int jump; + if (dj > 0) + jump = state.jpc->GetJumpPointUp(i, j, state.goal); + else + jump = state.jpc->GetJumpPointDown(i, j, state.goal); + + return jump; + } + else + { + ASSERT(dj == 1 || dj == -1); + int nj = j + dj; + while (true) + { + if (!PASSABLE(i, nj)) + return j; + + if (detectGoal && state.goal.NavcellContainsGoal(i, nj)) + { + state.open.clear(); + return nj; + } + +#if ACCEPT_DIAGONAL_GAPS + if ((!PASSABLE(i - 1, nj) && PASSABLE(i - 1, nj + dj)) || + (!PASSABLE(i + 1, nj) && PASSABLE(i + 1, nj + dj))) +#else + if ((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) || + (!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj))) +#endif + return nj; + + nj += dj; + } + } +} + +/* + * We never cache diagonal jump points - they're usually so frequent that + * a linear search is about as cheap and avoids the setup cost and memory cost. + */ +void LongPathfinder::AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderState& state) +{ + // ProcessNeighbour(i, j, i + di, j + dj, g, state); + // return; + + ASSERT(di == 1 || di == -1); + ASSERT(dj == 1 || dj == -1); + + int ni = i + di; + int nj = j + dj; + bool detectGoal = OnTheWay(i, j, di, dj, state.goal); + while (true) + { + // Stop if we hit an obstructed cell + if (!PASSABLE(ni, nj)) + return; + + // Stop if moving onto this cell caused us to + // touch the corner of an obstructed cell +#if !ACCEPT_DIAGONAL_GAPS + if (!PASSABLE(ni - di, nj) || !PASSABLE(ni, nj - dj)) + return; +#endif + + // Process this cell if it's at the goal + if (detectGoal && state.goal.NavcellContainsGoal(ni, nj)) + { + state.open.clear(); + ProcessNeighbour(i, j, ni, nj, g, state); + return; + } + +#if ACCEPT_DIAGONAL_GAPS + if ((!PASSABLE(ni - di, nj) && PASSABLE(ni - di, nj + dj)) || + (!PASSABLE(ni, nj - dj) && PASSABLE(ni + di, nj - dj))) + { + ProcessNeighbour(i, j, ni, nj, g, state); + return; + } +#endif + + int fi = HasJumpedHoriz(ni, nj, di, state, detectGoal ? OnTheWay(ni, nj, di, 0, state.goal) : false); + int fj = HasJumpedVert(ni, nj, dj, state, detectGoal ? OnTheWay(ni, nj, 0, dj, state.goal) : false); + if (fi != ni || fj != nj) + { + ProcessNeighbour(i, j, ni, nj, g, state); + g += PathCost::diag(abs(ni - i)); + + if (fi != ni) + ProcessNeighbour(ni, nj, fi, nj, g, state); + + if (fj != nj) + ProcessNeighbour(ni, nj, ni, fj, g, state); + + return; + } + + ni += di; + nj += dj; + } +} + +#undef PASSABLE + +void LongPathfinder::ComputeJPSPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path) +{ + PROFILE3("ComputePathJPS"); + + PathfinderState state = { 0 }; + + state.jpc = m_JumpPointCache[passClass].get(); + if (m_UseJPSCache && !state.jpc) + { + state.jpc = new JumpPointCache; + state.jpc->reset(m_Grid, passClass); + debug_printf("PATHFINDER: JPC memory: %d kB\n", (int)state.jpc->GetMemoryUsage() / 1024); + m_JumpPointCache[passClass] = shared_ptr(state.jpc); + } + + // Convert the start coordinates to tile indexes + u16 i0, j0; + Pathfinding::NearestNavcell(x0, z0, i0, j0, m_GridSize, m_GridSize); + + if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass)) + { + // The JPS pathfinder requires units to be on passable tiles + // (otherwise it might crash), so handle the supposedly-invalid + // state specially + m_PathfinderHier.FindNearestPassableNavcell(i0, j0, passClass); + } + + state.goal = origGoal; + + if (state.goal.type != PathGoal::POINT) + { + // Try to go to the nearest point on non-point goals. + // If it is reachable, use it, else fallback to the reachable point which is closest to the center of the goal + CFixedVector2D goalPos = state.goal.NearestPointOnGoal(CFixedVector2D(x0, z0)); + PathGoal pointGoal = { PathGoal::POINT, goalPos.X, goalPos.Y }; + if (!m_PathfinderHier.MakeGoalReachable(i0, j0, pointGoal, passClass)) + state.goal = pointGoal; + else + m_PathfinderHier.MakeGoalReachable(i0, j0, state.goal, passClass); + } + else + m_PathfinderHier.MakeGoalReachable(i0, j0, state.goal, passClass); + + // If we're already at the goal tile, then move directly to the exact goal coordinates + if (state.goal.NavcellContainsGoal(i0, j0)) + { + path.m_Waypoints.emplace_back(Waypoint{ state.goal.x, state.goal.z }); + return; + } + + Pathfinding::NearestNavcell(state.goal.x, state.goal.z, state.iGoal, state.jGoal, m_GridSize, m_GridSize); + + state.passClass = passClass; + + state.steps = 0; + + state.tiles = new PathfindTileGrid(m_Grid->m_W, m_Grid->m_H); + state.terrain = m_Grid; + + state.iBest = i0; + state.jBest = j0; + state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal); + + PriorityQueue::Item start = { TileID(i0, j0), PathCost() }; + 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).SetCost(PathCost()); + + while (true) + { + ++state.steps; + + // If we ran out of tiles to examine, give up + if (state.open.empty()) + break; + + // Move best tile from open to closed + PriorityQueue::Item curr = state.open.pop(); + u16 i = curr.id.i(); + u16 j = curr.id.j(); + state.tiles->get(i, j).SetStatusClosed(); + + // If we've reached the destination, stop + if (state.goal.NavcellContainsGoal(i, j)) + { + state.iBest = i; + state.jBest = j; + state.hBest = PathCost(); + break; + } + + PathfindTile tile = state.tiles->get(i, j); + PathCost g = tile.GetCost(); + + // Get the direction of the predecessor tile from this tile + int dpi = tile.GetPredDI(); + int dpj = tile.GetPredDJ(); + dpi = (dpi < 0 ? -1 : dpi > 0 ? 1 : 0); + dpj = (dpj < 0 ? -1 : dpj > 0 ? 1 : 0); + + if (dpi != 0 && dpj == 0) + { + // Moving horizontally from predecessor +#if ACCEPT_DIAGONAL_GAPS + if (!IS_PASSABLE(state.terrain->get(i, j-1), state.passClass)) + AddJumpedDiag(i, j, -dpi, -1, g, state); + if (!IS_PASSABLE(state.terrain->get(i, j+1), state.passClass)) + AddJumpedDiag(i, j, -dpi, +1, g, state); +#else + if (!IS_PASSABLE(state.terrain->get(i + dpi, j-1), state.passClass)) + { + AddJumpedDiag(i, j, -dpi, -1, g, state); + AddJumpedVert(i, j, -1, g, state, OnTheWay(i, j, 0, -1, state.goal)); + } + if (!IS_PASSABLE(state.terrain->get(i + dpi, j+1), state.passClass)) + { + AddJumpedDiag(i, j, -dpi, +1, g, state); + AddJumpedVert(i, j, +1, g, state, OnTheWay(i, j, 0, +1, state.goal)); + } +#endif + AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.goal)); + } + else if (dpi == 0 && dpj != 0) + { + // Moving vertically from predecessor +#if ACCEPT_DIAGONAL_GAPS + if (!IS_PASSABLE(state.terrain->get(i-1, j), state.passClass)) + AddJumpedDiag(i, j, -1, -dpj, g, state); + if (!IS_PASSABLE(state.terrain->get(i+1, j), state.passClass)) + AddJumpedDiag(i, j, +1, -dpj, g, state); +#else + if (!IS_PASSABLE(state.terrain->get(i-1, j + dpj), state.passClass)) + { + AddJumpedDiag(i, j, -1, -dpj, g, state); + AddJumpedHoriz(i, j, -1, g, state,OnTheWay(i, j, -1, 0, state.goal)); + } + if (!IS_PASSABLE(state.terrain->get(i+1, j + dpj), state.passClass)) + { + AddJumpedDiag(i, j, +1, -dpj, g, state); + AddJumpedHoriz(i, j, +1, g, state,OnTheWay(i, j, +1, 0, state.goal)); + } +#endif + AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.goal)); + } + else if (dpi != 0 && dpj != 0) + { + // Moving diagonally from predecessor +#if ACCEPT_DIAGONAL_GAPS + if (!IS_PASSABLE(state.terrain->get(i + dpi, j), state.passClass)) + AddJumpedDiag(i, j, dpi, -dpj, g, state); + if (!IS_PASSABLE(state.terrain->get(i, j + dpj), state.passClass)) + AddJumpedDiag(i, j, -dpi, dpj, g, state); +#endif + AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.goal)); + AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.goal)); + AddJumpedDiag(i, j, -dpi, -dpj, g, state); + } + else + { + // No predecessor, i.e. the start tile + // Start searching in every direction + + // XXX - check passability? + + bool passl = IS_PASSABLE(state.terrain->get(i-1, j), state.passClass); + bool passr = IS_PASSABLE(state.terrain->get(i+1, j), state.passClass); + bool passd = IS_PASSABLE(state.terrain->get(i, j-1), state.passClass); + bool passu = IS_PASSABLE(state.terrain->get(i, j+1), state.passClass); + + if (passl && passd) + ProcessNeighbour(i, j, i-1, j-1, g, state); + if (passr && passd) + ProcessNeighbour(i, j, i+1, j-1, g, state); + if (passl && passu) + ProcessNeighbour(i, j, i-1, j+1, g, state); + if (passr && passu) + ProcessNeighbour(i, j, i+1, j+1, g, state); + if (passl) + ProcessNeighbour(i, j, i-1, j, g, state); + if (passr) + ProcessNeighbour(i, j, i+1, j, g, state); + if (passd) + ProcessNeighbour(i, j, i, j-1, g, state); + if (passu) + 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; + Pathfinding::NavcellCenter(ip, jp, x, z); + path.m_Waypoints.emplace_back(Waypoint{ x, z }); + + // Follow the predecessor link + ip = n.GetPredI(ip); + jp = n.GetPredJ(jp); + } + // The last waypoint is slightly incorrect (it's not the goal but the center + // of the navcell of the goal), so replace it + if (!path.m_Waypoints.empty()) + path.m_Waypoints.front() = { state.goal.x, state.goal.z }; + + ImprovePathWaypoints(path, passClass, origGoal.maxdist, x0, z0); + + // Save this grid for debug display + delete m_DebugGrid; + m_DebugGrid = state.tiles; + m_DebugSteps = state.steps; + m_DebugGoal = state.goal; +} + +void LongPathfinder::ComputePathOffImpassable(entity_pos_t x0, entity_pos_t z0, const PathGoal& UNUSED(origGoal), pass_class_t passClass, WaypointPath& path) +{ + u16 i0, j0; + Pathfinding::NearestNavcell(x0, z0, i0, j0, m_GridSize, m_GridSize); + u16 iGoal = i0; + u16 jGoal = j0; + m_PathfinderHier.FindNearestPassableNavcell(iGoal, jGoal, passClass); + + int ip = iGoal; + int jp = jGoal; + + // Reconstruct the path (in reverse) + while (ip != i0 || jp != j0) + { + entity_pos_t x, z; + Pathfinding::NavcellCenter(ip, jp, x, z); + path.m_Waypoints.emplace_back(Waypoint{ x, z }); + + // Move diagonally/horizontally/vertically towards the start navcell + + if (ip > i0) + ip--; + else if (ip < i0) + ip++; + + if (jp > j0) + jp--; + else if (jp < j0) + jp++; + } +} + +void LongPathfinder::ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0) +{ + if (path.m_Waypoints.empty()) + return; + + if (maxDist > fixed::Zero()) + { + CFixedVector2D start(x0, z0); + CFixedVector2D first(path.m_Waypoints.back().x, path.m_Waypoints.back().z); + CFixedVector2D offset = first - start; + if (offset.CompareLength(maxDist) > 0) + { + offset.Normalize(maxDist); + path.m_Waypoints.emplace_back(Waypoint{ (start + offset).X, (start + offset).Y }); + } + } + + if (path.m_Waypoints.size() < 2) + return; + + std::vector& waypoints = path.m_Waypoints; + std::vector newWaypoints; + + CFixedVector2D prev(waypoints.front().x, waypoints.front().z); + newWaypoints.push_back(waypoints.front()); + for (size_t k = 1; k < waypoints.size() - 1; ++k) + { + CFixedVector2D ahead(waypoints[k + 1].x, waypoints[k + 1].z); + CFixedVector2D curr(waypoints[k].x, waypoints[k].z); + + if (maxDist > fixed::Zero() && (curr - prev).CompareLength(maxDist) > 0) + { + // We are too far away from the previous waypoint, so create one in + // between and continue with the improvement of the path + prev = prev + (curr - prev) / 2; + newWaypoints.emplace_back(Waypoint{ prev.X, prev.Y }); + } + + // If we're mostly straight, don't even bother. + if ((ahead - curr).Perpendicular().Dot(curr - prev).Absolute() <= fixed::Epsilon() * 100) + continue; + + if (!CheckLineMovement(prev.X, prev.Y, ahead.X, ahead.Y, passClass)) + { + prev = CFixedVector2D(waypoints[k].x, waypoints[k].z); + newWaypoints.push_back(waypoints[k]); + } + } + newWaypoints.push_back(waypoints.back()); + path.m_Waypoints.swap(newWaypoints); +} + +bool LongPathfinder::CheckLineMovement(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, pass_class_t passClass) +{ + // We shouldn't allow lines between diagonally-adjacent navcells. + // It doesn't matter whether we allow lines precisely along the edge + // of an impassable navcell. + + // To rasterise the line: + // If the line is (e.g.) aiming up-right, then we start at the navcell + // containing the start point and the line must either end in that navcell + // or else exit along the top edge or the right edge (or through the top-right corner, + // which we'll arbitrary treat as the horizontal edge). + // So we jump into the adjacent navcell across that edge, and continue. + + // To handle the special case of units that are stuck on impassable cells, + // we allow them to move from an impassable to a passable cell (but not + // vice versa). + + u16 i0, j0, i1, j1; + Pathfinding::NearestNavcell(x0, z0, i0, j0, m_GridSize, m_GridSize); + Pathfinding::NearestNavcell(x1, z1, i1, j1, m_GridSize, m_GridSize); + + // Find which direction the line heads in + int di = (i0 < i1 ? +1 : i1 < i0 ? -1 : 0); + int dj = (j0 < j1 ? +1 : j1 < j0 ? -1 : 0); + + u16 i = i0; + u16 j = j0; + + bool currentlyOnImpassable = !IS_PASSABLE(m_Grid->get(i0, j0), passClass); + + while (true) + { + // Fail if we're moving onto an impassable navcell + bool passable = IS_PASSABLE(m_Grid->get(i, j), passClass); + if (passable) + currentlyOnImpassable = false; + else if (!currentlyOnImpassable) + return false; + + // Succeed if we're at the target + if (i == i1 && j == j1) + return true; + + // If we can only move horizontally/vertically, then just move in that direction + if (di == 0) + { + j += dj; + continue; + } + else if (dj == 0) + { + i += di; + continue; + } + + // Otherwise we need to check which cell to move into: + + // Check whether the line intersects the horizontal (top/bottom) edge of + // the current navcell. + // Horizontal edge is (i, j + (dj>0?1:0)) .. (i + 1, j + (dj>0?1:0)) + // Since we already know the line is moving from this navcell into a different + // navcell, we simply need to test that the edge's endpoints are not both on the + // same side of the line. + + entity_pos_t xia = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t xib = entity_pos_t::FromInt(i+1).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t zj = entity_pos_t::FromInt(j + (dj+1)/2).Multiply(Pathfinding::NAVCELL_SIZE); + + CFixedVector2D perp = CFixedVector2D(x1 - x0, z1 - z0).Perpendicular(); + entity_pos_t dota = (CFixedVector2D(xia, zj) - CFixedVector2D(x0, z0)).Dot(perp); + entity_pos_t dotb = (CFixedVector2D(xib, zj) - CFixedVector2D(x0, z0)).Dot(perp); + + // If the horizontal edge is fully on one side of the line, so the line doesn't + // intersect it, we should move across the vertical edge instead + if ((dota < entity_pos_t::Zero() && dotb < entity_pos_t::Zero()) || + (dota > entity_pos_t::Zero() && dotb > entity_pos_t::Zero())) + i += di; + else + j += dj; + } +} + +void LongPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid& grid) +{ + steps = m_DebugSteps; + time = m_DebugTime; + + if (!m_DebugGrid) + return; + + u16 iGoal, jGoal; + Pathfinding::NearestNavcell(m_DebugGoal.x, m_DebugGoal.z, iGoal, jGoal, m_GridSize, m_GridSize); + + grid = Grid(m_DebugGrid->m_W, m_DebugGrid->m_H); + for (u16 j = 0; j < grid.m_H; ++j) + { + for (u16 i = 0; i < grid.m_W; ++i) + { + if (i == iGoal && j == jGoal) + continue; + PathfindTile t = m_DebugGrid->get(i, j); + grid.set(i, j, (t.IsOpen() ? 1 : 0) | (t.IsClosed() ? 2 : 0)); + } + } +} + +void LongPathfinder::SetDebugOverlay(bool enabled) +{ + if (enabled && !m_DebugOverlay) + m_DebugOverlay = new LongOverlay(*this); + else if (!enabled && m_DebugOverlay) + SAFE_DELETE(m_DebugOverlay); +} + +void LongPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, + pass_class_t passClass, std::vector excludedRegions, WaypointPath& path) +{ + GenerateSpecialMap(passClass, excludedRegions); + ComputePath(x0, z0, origGoal, SPECIAL_PASS_CLASS, path); +} + +inline bool InRegion(u16 i, u16 j, CircularRegion region) +{ + fixed cellX = Pathfinding::NAVCELL_SIZE * i; + fixed cellZ = Pathfinding::NAVCELL_SIZE * j; + + return CFixedVector2D(cellX - region.x, cellZ - region.z).CompareLength(region.r) <= 0; +} + +void LongPathfinder::GenerateSpecialMap(pass_class_t passClass, std::vector excludedRegions) +{ + for (u16 j = 0; j < m_Grid->m_H; ++j) + { + for (u16 i = 0; i < m_Grid->m_W; ++i) + { + NavcellData n = m_Grid->get(i, j); + if (!IS_PASSABLE(n, passClass)) + { + n |= SPECIAL_PASS_CLASS; + m_Grid->set(i, j, n); + continue; + } + + for (CircularRegion& region : excludedRegions) + { + if (!InRegion(i, j, region)) + continue; + + n |= SPECIAL_PASS_CLASS; + break; + } + m_Grid->set(i, j, n); + } + } +} diff --git a/source/simulation2/helpers/LongPathfinder.h b/source/simulation2/helpers/LongPathfinder.h new file mode 100644 index 0000000000..34ddbe84f7 --- /dev/null +++ b/source/simulation2/helpers/LongPathfinder.h @@ -0,0 +1,424 @@ +/* Copyright (C) 2015 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 . + */ + +#ifndef INCLUDED_LONGPATHFINDER +#define INCLUDED_LONGPATHFINDER + +#include "Pathfinding.h" +#include "HierarchicalPathfinder.h" + +#include "PriorityQueue.h" +#include "graphics/Overlay.h" +#include "renderer/Scene.h" + +#define ACCEPT_DIAGONAL_GAPS 0 + +/** + * Represents the 2D coordinates of a tile. + * The i/j components are packed into a single u32, since we usually use these + * objects for equality comparisons and the VC2010 optimizer doesn't seem to automatically + * compare two u16s in a single operation. + * TODO: maybe VC2012 will? + */ +struct TileID +{ + TileID() { } + + TileID(u16 i, u16 j) : data((i << 16) | j) { } + + bool operator==(const TileID& b) const + { + return data == b.data; + } + + /// Returns lexicographic order over (i,j) + bool operator<(const TileID& b) const + { + return data < b.data; + } + + u16 i() const { return data >> 16; } + u16 j() const { return data & 0xFFFF; } + +private: + u32 data; +}; + +/** + * Tile data for A* computation. + * (We store an array of one of these per terrain tile, so it ought to be optimised for size) + */ +struct PathfindTile +{ +public: + enum { + STATUS_UNEXPLORED = 0, + STATUS_OPEN = 1, + STATUS_CLOSED = 2 + }; + + bool IsUnexplored() { return GetStatus() == STATUS_UNEXPLORED; } + bool IsOpen() { return GetStatus() == STATUS_OPEN; } + bool IsClosed() { return GetStatus() == STATUS_CLOSED; } + void SetStatusOpen() { SetStatus(STATUS_OPEN); } + void SetStatusClosed() { SetStatus(STATUS_CLOSED); } + + // Get pi,pj coords of predecessor to this tile on best path, given i,j coords of this tile + inline int GetPredI(int i) { return i + GetPredDI(); } + inline int GetPredJ(int j) { return j + GetPredDJ(); } + + inline PathCost GetCost() const { return g; } + inline void SetCost(PathCost cost) { g = cost; } + +private: + PathCost g; // cost to reach this tile + u32 data; // 2-bit status; 15-bit PredI; 15-bit PredJ; packed for storage efficiency + +public: + inline u8 GetStatus() const + { + return data & 3; + } + + inline void SetStatus(u8 s) + { + ASSERT(s < 4); + data &= ~3; + data |= (s & 3); + } + + int GetPredDI() const + { + return (i32)data >> 17; + } + + int GetPredDJ() const + { + return ((i32)data << 15) >> 17; + } + + // Set the pi,pj coords of predecessor, given i,j coords of this tile + inline void SetPred(int pi, int pj, int i, int j) + { + int di = pi - i; + int dj = pj - j; + ASSERT(-16384 <= di && di < 16384); + ASSERT(-16384 <= dj && dj < 16384); + data &= 3; + data |= (((u32)di & 0x7FFF) << 17) | (((u32)dj & 0x7FFF) << 2); + } +}; + +struct CircularRegion +{ + CircularRegion(entity_pos_t x, entity_pos_t z, entity_pos_t r) : x(x), z(z), r(r) {} + entity_pos_t x, z, r; +}; + +typedef PriorityQueueHeap PriorityQueue; +typedef SparseGrid PathfindTileGrid; + +class JumpPointCache; + +struct PathfinderState +{ + u32 steps; // number of algorithm iterations + + PathGoal goal; + + u16 iGoal, jGoal; // goal tile + + pass_class_t passClass; + + PriorityQueue open; + // (there's no explicit closed list; it's encoded in PathfindTile) + + PathfindTileGrid* tiles; + Grid* terrain; + + PathCost hBest; // heuristic of closest discovered tile to goal + u16 iBest, jBest; // closest tile + + JumpPointCache* jpc; +}; + +class LongOverlay; + +class LongPathfinder +{ +public: + LongPathfinder() + { + m_UseJPSCache = false; + + m_Grid = NULL; + m_GridSize = 0; + + m_DebugOverlay = NULL; + m_DebugGrid = NULL; + m_DebugPath = NULL; + } + + ~LongPathfinder() + { + SAFE_DELETE(m_DebugOverlay); + SAFE_DELETE(m_DebugGrid); + SAFE_DELETE(m_DebugPath); + } + + void SetDebugOverlay(bool enabled); + + void SetHierDebugOverlay(bool enabled, const CSimContext *simContext) + { + m_PathfinderHier.SetDebugOverlay(enabled, simContext); + } + + void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass) + { + if (!m_DebugOverlay) + return; + + SAFE_DELETE(m_DebugGrid); + delete m_DebugPath; + m_DebugPath = new WaypointPath(); + ComputePath(x0, z0, goal, passClass, *m_DebugPath); + m_DebugPassClass = passClass; + } + + void Reload(std::map passClassMasks, Grid* passabilityGrid) + { + m_Grid = passabilityGrid; + ASSERT(passabilityGrid->m_H == passabilityGrid->m_W); + m_GridSize = passabilityGrid->m_W; + + m_JumpPointCache.clear(); + + m_PathfinderHier.Recompute(passClassMasks, passabilityGrid); + } + + void Update(Grid* passabilityGrid, const Grid* dirtinessGrid) + { + m_Grid = passabilityGrid; + ASSERT(passabilityGrid->m_H == passabilityGrid->m_W); + ASSERT(m_GridSize == passabilityGrid->m_H); + + m_JumpPointCache.clear(); + + m_PathfinderHier.Update(passabilityGrid, dirtinessGrid); + } + + void HierarchicalRenderSubmit(SceneCollector& collector) + { + for (size_t i = 0; i < m_PathfinderHier.m_DebugOverlayLines.size(); ++i) + collector.Submit(&m_PathfinderHier.m_DebugOverlayLines[i]); + } + + /** + * Compute a tile-based path from the given point to the goal, and return the set of waypoints. + * The waypoints correspond to the centers of horizontally/vertically adjacent tiles + * along the path. + */ + void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, + pass_class_t passClass, WaypointPath& path) + { + if (!m_Grid) + { + LOGERROR("The pathfinder grid hasn't been setup yet, aborting ComputePath"); + return; + } + + ComputeJPSPath(x0, z0, origGoal, passClass, path); + } + + /** + * Compute a tile-based path from the given point to the goal, excluding the regions + * specified in excludedRegions (which are treated as impassable) and return the set of waypoints. + * The waypoints correspond to the centers of horizontally/vertically adjacent tiles + * along the path. + */ + void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, + pass_class_t passClass, std::vector excludedRegions, WaypointPath& path); + + Grid GetConnectivityGrid(pass_class_t passClass) + { + return m_PathfinderHier.GetConnectivityGrid(passClass); + } + + void GetDebugData(u32& steps, double& time, Grid& grid) + { + GetDebugDataJPS(steps, time, grid); + } + + /* + * Checks that the line (x0,z0)-(x1,z1) does not intersect any impassable navcells. + */ + bool CheckLineMovement(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, pass_class_t passClass); + + Grid* m_Grid; + u16 m_GridSize; + + // Debugging - output from last pathfind operation: + LongOverlay* m_DebugOverlay; + PathfindTileGrid* m_DebugGrid; + u32 m_DebugSteps; + double m_DebugTime; + PathGoal m_DebugGoal; + WaypointPath* m_DebugPath; + pass_class_t m_DebugPassClass; + +private: + PathCost CalculateHeuristic(int i, int j, int iGoal, int jGoal); + void ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state); + + /** + * JPS algorithm helper functions + * @param detectGoal is not used if m_UseJPSCache is true + */ + void AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal); + int HasJumpedHoriz(int i, int j, int di, PathfinderState& state, bool detectGoal); + void AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal); + int HasJumpedVert(int i, int j, int dj, PathfinderState& state, bool detectGoal); + void AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderState& state); + + /** + * See LongPathfinder.cpp for implementation details + * TODO: cleanup documentation + */ + void ComputeJPSPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path); + void GetDebugDataJPS(u32& steps, double& time, Grid& grid); + + // Helper functions for ComputePath + + /** + * Same kind of interface as ComputePath, but works when the unit is starting + * on an impassable navcell. + * Returns a path heading directly to the nearest passable navcell, then the goal. + */ + void ComputePathOffImpassable(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path); + + /** + * Given a path with an arbitrary collection of waypoints, updates the + * waypoints to be nicer. Calls "Testline" between waypoints + * so that bended paths can become straight if there's nothing in between + * (this happens because A* is 8-direction, and the map isn't actually a grid). + * If @param maxDist is non-zero, path waypoints will be espaced by at most @param maxDist. + * In that case the distance between (x0, z0) and the first waypoint will also be made less than maxDist. + */ + void ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0); + + /** + * Generate a passability map, stored in the 16th bit of navcells, based on passClass, + * but with a set of impassable circular regions. + */ + void GenerateSpecialMap(pass_class_t passClass, std::vector excludedRegions); + + bool m_UseJPSCache; + std::map > m_JumpPointCache; + + HierarchicalPathfinder m_PathfinderHier; +}; + +/** + * Terrain overlay for pathfinder debugging. + * Renders a representation of the most recent pathfinding operation. + */ +class LongOverlay : public TerrainTextureOverlay +{ +public: + LongPathfinder& m_Pathfinder; + + LongOverlay(LongPathfinder& pathfinder) : + TerrainTextureOverlay(Pathfinding::NAVCELLS_PER_TILE), m_Pathfinder(pathfinder) + { + } + + virtual void BuildTextureRGBA(u8* data, size_t w, size_t h) + { + // Grab the debug data for the most recently generated path + u32 steps; + double time; + Grid debugGrid; + m_Pathfinder.GetDebugData(steps, time, debugGrid); + + // Render navcell passability + u8* p = data; + for (size_t j = 0; j < h; ++j) + { + for (size_t i = 0; i < w; ++i) + { + SColor4ub color(0, 0, 0, 0); + if (!IS_PASSABLE(m_Pathfinder.m_Grid->get((int)i, (int)j), m_Pathfinder.m_DebugPassClass)) + color = SColor4ub(255, 0, 0, 127); + + if (debugGrid.m_W && debugGrid.m_H) + { + u8 n = debugGrid.get((int)i, (int)j); + + if (n == 1) + color = SColor4ub(255, 255, 0, 127); + else if (n == 2) + color = SColor4ub(0, 255, 0, 127); + + if (m_Pathfinder.m_DebugGoal.NavcellContainsGoal(i, j)) + color = SColor4ub(0, 0, 255, 127); + } + + *p++ = color.R; + *p++ = color.G; + *p++ = color.B; + *p++ = color.A; + } + } + + // Render the most recently generated path + if (m_Pathfinder.m_DebugPath && !m_Pathfinder.m_DebugPath->m_Waypoints.empty()) + { + std::vector& waypoints = m_Pathfinder.m_DebugPath->m_Waypoints; + u16 ip = 0, jp = 0; + for (size_t k = 0; k < waypoints.size(); ++k) + { + u16 i, j; + Pathfinding::NearestNavcell(waypoints[k].x, waypoints[k].z, i, j, m_Pathfinder.m_GridSize, m_Pathfinder.m_GridSize); + if (k == 0) + { + ip = i; + jp = j; + } + else + { + bool firstCell = true; + do + { + if (data[(jp*w + ip)*4+3] == 0) + { + data[(jp*w + ip)*4+0] = 0xFF; + data[(jp*w + ip)*4+1] = 0xFF; + data[(jp*w + ip)*4+2] = 0xFF; + data[(jp*w + ip)*4+3] = firstCell ? 0xA0 : 0x60; + } + ip = ip < i ? ip+1 : ip > i ? ip-1 : ip; + jp = jp < j ? jp+1 : jp > j ? jp-1 : jp; + firstCell = false; + } + while (ip != i || jp != j); + } + } + } + } +}; + +#endif // INCLUDED_LONGPATHFINDER diff --git a/source/simulation2/helpers/PathGoal.cpp b/source/simulation2/helpers/PathGoal.cpp new file mode 100644 index 0000000000..8b4e84d75a --- /dev/null +++ b/source/simulation2/helpers/PathGoal.cpp @@ -0,0 +1,302 @@ +/* Copyright (C) 2015 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 "PathGoal.h" + +#include "graphics/Terrain.h" +#include "Pathfinding.h" + +static bool NavcellContainsCircle(int i, int j, fixed x, fixed z, fixed r, bool inside) +{ + // Accept any navcell (i,j) that contains a point which is inside[/outside] + // (or on the edge of) the circle + + // Get world-space bounds of navcell + entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + + if (inside) + { + // Get the point inside the navcell closest to (x,z) + entity_pos_t nx = Clamp(x, x0, x1); + entity_pos_t nz = Clamp(z, z0, z1); + // Check if that point is inside the circle + return (CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(r) <= 0; + } + else + { + // If any corner of the navcell is outside the circle, return true. + // Otherwise, since the circle is convex, there cannot be any other point + // in the navcell that is outside the circle. + return ( + (CFixedVector2D(x0, z0) - CFixedVector2D(x, z)).CompareLength(r) >= 0 + || (CFixedVector2D(x1, z0) - CFixedVector2D(x, z)).CompareLength(r) >= 0 + || (CFixedVector2D(x0, z1) - CFixedVector2D(x, z)).CompareLength(r) >= 0 + || (CFixedVector2D(x1, z1) - CFixedVector2D(x, z)).CompareLength(r) >= 0 + ); + } +} + +static bool NavcellContainsSquare(int i, int j, + fixed x, fixed z, CFixedVector2D u, CFixedVector2D v, fixed hw, fixed hh, + bool inside) +{ + // Accept any navcell (i,j) that contains a point which is inside[/outside] + // (or on the edge of) the square + + // Get world-space bounds of navcell + entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + + if (inside) + { + // Get the point inside the navcell closest to (x,z) + entity_pos_t nx = Clamp(x, x0, x1); + entity_pos_t nz = Clamp(z, z0, z1); + // Check if that point is inside the circle + return Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh)); + } + else + { + // If any corner of the navcell is outside the square, return true. + // Otherwise, since the square is convex, there cannot be any other point + // in the navcell that is outside the square. + return ( + Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z0 - z), u, v, CFixedVector2D(hw, hh)) + || Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z0 - z), u, v, CFixedVector2D(hw, hh)) + || Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z1 - z), u, v, CFixedVector2D(hw, hh)) + || Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z1 - z), u, v, CFixedVector2D(hw, hh)) + ); + } +} + +bool PathGoal::NavcellContainsGoal(int i, int j) const +{ + switch (type) + { + case POINT: + { + // Only accept a single navcell + int gi = (x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity(); + int gj = (z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity(); + return gi == i && gj == j; + } + case CIRCLE: + return NavcellContainsCircle(i, j, x, z, hw, true); + case INVERTED_CIRCLE: + return NavcellContainsCircle(i, j, x, z, hw, false); + case SQUARE: + return NavcellContainsSquare(i, j, x, z, u, v, hw, hh, true); + case INVERTED_SQUARE: + return NavcellContainsSquare(i, j, x, z, u, v, hw, hh, false); + NODEFAULT; + } +} + +bool PathGoal::NavcellRectContainsGoal(int i0, int j0, int i1, int j1, int* gi, int* gj) const +{ + // Get min/max to simplify range checks + int imin = std::min(i0, i1); + int imax = std::max(i0, i1); + int jmin = std::min(j0, j1); + int jmax = std::max(j0, j1); + + // Direction to iterate from ij0 towards ij1 + int di = i1 < i0 ? -1 : +1; + int dj = j1 < j0 ? -1 : +1; + + switch (type) + { + case POINT: + { + // Calculate the navcell that contains the point goal + int i = (x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity(); + int j = (z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity(); + // If that goal navcell is in the given range, return it + if (imin <= i && i <= imax && jmin <= j && j <= jmax) + { + if (gi) + *gi = i; + if (gj) + *gj = j; + return true; + } + return false; + } + + case CIRCLE: + { + // Loop over all navcells in the given range (starting at (i0,j0) since + // this function is meant to find the goal navcell nearest to there + // assuming jmin==jmax || imin==imax), + // and check whether any point in each navcell is within the goal circle. + // (TODO: this is pretty inefficient.) + for (int j = j0; jmin <= j && j <= jmax; j += dj) + { + for (int i = i0; imin <= i && i <= imax; i += di) + { + entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t nx = Clamp(x, x0, x1); + entity_pos_t nz = Clamp(z, z0, z1); + if ((CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(hw) <= 0) + { + if (gi) + *gi = i; + if (gj) + *gj = j; + return true; + } + } + } + return false; + } + + case SQUARE: + { + // Loop over all navcells in the given range (starting at (i0,j0) since + // this function is meant to find the goal navcell nearest to there + // assuming jmin==jmax || imin==imax), + // and check whether any point in each navcell is within the goal square. + // (TODO: this is pretty inefficient.) + for (int j = j0; jmin <= j && j <= jmax; j += dj) + { + for (int i = i0; imin <= i && i <= imax; i += di) + { + entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t nx = Clamp(x, x0, x1); + entity_pos_t nz = Clamp(z, z0, z1); + if (Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh))) + { + if (gi) + *gi = i; + if (gj) + *gj = j; + return true; + } + } + } + return false; + } + + case INVERTED_CIRCLE: + case INVERTED_SQUARE: + // Haven't bothered implementing these, since they're not needed by the + // current pathfinder design + debug_warn(L"PathGoal::NavcellRectContainsGoal doesn't support inverted shapes"); + return false; + + NODEFAULT; + } +} + +bool PathGoal::RectContainsGoal(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1) const +{ + switch (type) + { + case POINT: + return x0 <= x && x <= x1 && z0 <= z && z <= z1; + + case CIRCLE: + { + entity_pos_t nx = Clamp(x, x0, x1); + entity_pos_t nz = Clamp(z, z0, z1); + return (CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(hw) <= 0; + } + + case SQUARE: + { + entity_pos_t nx = Clamp(x, x0, x1); + entity_pos_t nz = Clamp(z, z0, z1); + return Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh)); + } + + case INVERTED_CIRCLE: + case INVERTED_SQUARE: + // Haven't bothered implementing these, since they're not needed by the + // current pathfinder design + debug_warn(L"PathGoal::RectContainsGoal doesn't support inverted shapes"); + return false; + + NODEFAULT; + } +} + +fixed PathGoal::DistanceToPoint(CFixedVector2D pos) const +{ + switch (type) + { + case PathGoal::POINT: + return (pos - CFixedVector2D(x, z)).Length(); + + case PathGoal::CIRCLE: + case PathGoal::INVERTED_CIRCLE: + return ((pos - CFixedVector2D(x, z)).Length() - hw).Absolute(); + + case PathGoal::SQUARE: + case PathGoal::INVERTED_SQUARE: + { + CFixedVector2D halfSize(hw, hh); + CFixedVector2D d(pos.X - x, pos.Y - z); + return Geometry::DistanceToSquare(d, u, v, halfSize); + } + + NODEFAULT; + } +} + +CFixedVector2D PathGoal::NearestPointOnGoal(CFixedVector2D pos) const +{ + CFixedVector2D g(x, z); + + switch (type) + { + case PathGoal::POINT: + return g; + + case PathGoal::CIRCLE: + case PathGoal::INVERTED_CIRCLE: + { + CFixedVector2D d = pos - g; + if (d.IsZero()) + d = CFixedVector2D(fixed::FromInt(1), fixed::Zero()); // some arbitrary direction + d.Normalize(hw); + return g + d; + } + + case PathGoal::SQUARE: + case PathGoal::INVERTED_SQUARE: + { + CFixedVector2D halfSize(hw, hh); + CFixedVector2D d = pos - g; + return g + Geometry::NearestPointOnSquare(d, u, v, halfSize); + } + + NODEFAULT; + } +} diff --git a/source/simulation2/helpers/PathGoal.h b/source/simulation2/helpers/PathGoal.h new file mode 100644 index 0000000000..b2ba9f2911 --- /dev/null +++ b/source/simulation2/helpers/PathGoal.h @@ -0,0 +1,84 @@ +/* Copyright (C) 2015 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 . + */ + +#ifndef INCLUDED_PATHGOAL +#define INCLUDED_PATHGOAL + +#include "maths/FixedVector2D.h" +#include "simulation2/helpers/Position.h" + +/** + * Pathfinder goal. + * The goal can be either a point, a circle, or a square (rectangle). + * For circles/squares, any point inside the shape is considered to be + * part of the goal. + * Also, it can be an 'inverted' circle/square, where any point outside + * the shape is part of the goal. + */ +class PathGoal +{ +public: + enum Type { + POINT, // single point + CIRCLE, // the area inside a circle + INVERTED_CIRCLE, // the area outside a circle + SQUARE, // the area inside a square + INVERTED_SQUARE // the area outside a square + } type; + + entity_pos_t x, z; // position of center + + CFixedVector2D u, v; // if [INVERTED_]SQUARE, then orthogonal unit axes + + entity_pos_t hw, hh; // if [INVERTED_]SQUARE, then half width & height; if [INVERTED_]CIRCLE, then hw is radius + + entity_pos_t maxdist; // maximum distance wanted between two path waypoints + + /** + * Returns true if the given navcell contains a part of the goal area. + */ + bool NavcellContainsGoal(int i, int j) const; + + /** + * Returns true if any navcell (i, j) where + * min(i0,i1) <= i <= max(i0,i1) + * min(j0,j1) <= j <= max(j0,j1), + * contains a part of the goal area. + * If so, arguments i and j (if not NULL) are set to the goal navcell nearest + * to (i0, j0), assuming the rect has either width or height = 1. + */ + bool NavcellRectContainsGoal(int i0, int j0, int i1, int j1, int* i, int* j) const; + + /** + * Returns true if the rectangle defined by (x0,z0)-(x1,z1) (inclusive) + * contains a part of the goal area. + */ + bool RectContainsGoal(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1) const; + + /** + * Returns the minimum distance from the point with the given @p pos + * to any point on the outline of the goal shape. + */ + fixed DistanceToPoint(CFixedVector2D pos) const; + + /** + * Returns the coordinates of the point on the goal that is closest to pos in a straight line. + */ + CFixedVector2D NearestPointOnGoal(CFixedVector2D pos) const; +}; + +#endif // INCLUDED_PATHGOAL diff --git a/source/simulation2/helpers/Pathfinding.h b/source/simulation2/helpers/Pathfinding.h new file mode 100644 index 0000000000..ca530b89b7 --- /dev/null +++ b/source/simulation2/helpers/Pathfinding.h @@ -0,0 +1,241 @@ +/* Copyright (C) 2015 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 . + */ + +#ifndef INCLUDED_PATHFINDING +#define INCLUDED_PATHFINDING + +#include "ps/CLogger.h" + +#include "simulation2/system/ParamNode.h" +#include "graphics/Terrain.h" +#include "Geometry.h" +#include "Grid.h" +#include "PathGoal.h" + +typedef u16 pass_class_t; + +struct Waypoint +{ + entity_pos_t x, z; +}; + +/** + * Returned path. + * Waypoints are in *reverse* order (the earliest is at the back of the list) + */ +struct WaypointPath +{ + std::vector m_Waypoints; +}; + +/** + * Represents the cost of a path consisting of horizontal/vertical and + * diagonal movements over a uniform-cost grid. + * Maximum path length before overflow is about 45K steps. + */ +struct PathCost +{ + PathCost() : data(0) { } + + /// Construct from a number of horizontal/vertical and diagonal steps + PathCost(u16 hv, u16 d) + : data(hv * 65536 + d * 92682) // 2^16 * sqrt(2) == 92681.9 + { + } + + /// Construct for horizontal/vertical movement of given number of steps + static PathCost horizvert(u16 n) + { + return PathCost(n, 0); + } + + /// Construct for diagonal movement of given number of steps + static PathCost diag(u16 n) + { + return PathCost(0, n); + } + + PathCost operator+(const PathCost& a) const + { + PathCost c; + c.data = data + a.data; + return c; + } + + PathCost& operator+=(const PathCost& a) + { + data += a.data; + return *this; + } + + bool operator<=(const PathCost& b) const { return data <= b.data; } + bool operator< (const PathCost& b) const { return data < b.data; } + bool operator>=(const PathCost& b) const { return data >= b.data; } + bool operator>(const PathCost& b) const { return data > b.data; } + + u32 ToInt() + { + return data; + } + +private: + u32 data; +}; + +typedef u16 NavcellData; // 1 bit per passability class (up to PASS_CLASS_BITS) +static const int PASS_CLASS_BITS = 16; +#define IS_PASSABLE(item, classmask) (((item) & (classmask)) == 0) +#define PASS_CLASS_MASK_FROM_INDEX(id) ((pass_class_t)(1u << id)) +#define SPECIAL_PASS_CLASS PASS_CLASS_MASK_FROM_INDEX(PASS_CLASS_BITS) // 16th bit, used for special in-place computations + +namespace Pathfinding +{ + /** + * The pathfinders operate primarily over a navigation grid (a uniform-cost + * 2D passability grid, with horizontal/vertical (not diagonal) connectivity). + * This is based on the terrain tile passability, plus the rasterized shapes of + * obstructions, all expanded outwards by the radius of the units. + * Since units are much smaller than terrain tiles, the nav grid should be + * higher resolution than the tiles. + * We therefore split each terrain tile into NxN "nav cells" (for some integer N, + * preferably a power of two). + */ + const int NAVCELLS_PER_TILE = 4; + + /** + * Size of a navcell in metres ( = TERRAIN_TILE_SIZE / NAVCELLS_PER_TILE) + */ + extern const fixed NAVCELL_SIZE; + const int NAVCELL_SIZE_INT = 1; + const int NAVCELL_SIZE_LOG2 = 0; + + /** + * Compute the navcell indexes on the grid nearest to a given point + * w, h are the grid dimensions, i.e. the number of navcells per side + */ + inline void NearestNavcell(entity_pos_t x, entity_pos_t z, u16& i, u16& j, u16 w, u16 h) + { + i = (u16)clamp((x / NAVCELL_SIZE).ToInt_RoundToNegInfinity(), 0, w - 1); + j = (u16)clamp((z / NAVCELL_SIZE).ToInt_RoundToNegInfinity(), 0, h - 1); + } + + /** + * Returns the position of the center of the given tile + */ + inline void TileCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z) + { + cassert(TERRAIN_TILE_SIZE % 2 == 0); + x = entity_pos_t::FromInt(i*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE / 2); + z = entity_pos_t::FromInt(j*(int)TERRAIN_TILE_SIZE + (int)TERRAIN_TILE_SIZE / 2); + } + + inline void NavcellCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z) + { + x = entity_pos_t::FromInt(i * 2 + 1).Multiply(NAVCELL_SIZE / 2); + z = entity_pos_t::FromInt(j * 2 + 1).Multiply(NAVCELL_SIZE / 2); + } +} + +/* + * For efficient pathfinding we want to try hard to minimise the per-tile search cost, + * so we precompute the tile passability flags and movement costs for the various different + * types of unit. + * We also want to minimise memory usage (there can easily be 100K tiles so we don't want + * to store many bytes for each). + * + * To handle passability efficiently, we have a small number of passability classes + * (e.g. "infantry", "ship"). Each unit belongs to a single passability class, and + * uses that for all its pathfinding. + * Passability is determined by water depth, terrain slope, forestness, buildingness. + * We need at least one bit per class per tile to represent passability. + * + * We use a separate bit to indicate building obstructions (instead of folding it into + * the class passabilities) so that it can be ignored when doing the accurate short paths. + * We use another bit to indicate tiles near obstructions that block construction, + * for the AI to plan safe building spots. + */ +class PathfinderPassability +{ +public: + PathfinderPassability(pass_class_t mask, const CParamNode& node) : + m_Mask(mask) + { + if (node.GetChild("MinWaterDepth").IsOk()) + m_MinDepth = node.GetChild("MinWaterDepth").ToFixed(); + else + m_MinDepth = std::numeric_limits::min(); + + if (node.GetChild("MaxWaterDepth").IsOk()) + m_MaxDepth = node.GetChild("MaxWaterDepth").ToFixed(); + else + m_MaxDepth = std::numeric_limits::max(); + + if (node.GetChild("MaxTerrainSlope").IsOk()) + m_MaxSlope = node.GetChild("MaxTerrainSlope").ToFixed(); + else + m_MaxSlope = std::numeric_limits::max(); + + if (node.GetChild("MinShoreDistance").IsOk()) + m_MinShore = node.GetChild("MinShoreDistance").ToFixed(); + else + m_MinShore = std::numeric_limits::min(); + + if (node.GetChild("MaxShoreDistance").IsOk()) + m_MaxShore = node.GetChild("MaxShoreDistance").ToFixed(); + else + m_MaxShore = std::numeric_limits::max(); + + if (node.GetChild("Clearance").IsOk()) + { + m_HasClearance = true; + m_Clearance = node.GetChild("Clearance").ToFixed(); + + if (!(m_Clearance % Pathfinding::NAVCELL_SIZE).IsZero()) + { + // If clearance isn't an integer number of navcells then we'll + // probably get weird behaviour when expanding the navcell grid + // by clearance, vs expanding static obstructions by clearance + LOGWARNING("Pathfinder passability class has clearance %f, should be multiple of %f", + m_Clearance.ToFloat(), Pathfinding::NAVCELL_SIZE.ToFloat()); + } + } + else + { + m_HasClearance = false; + m_Clearance = fixed::Zero(); + } + } + + bool IsPassable(fixed waterdepth, fixed steepness, fixed shoredist) + { + return ((m_MinDepth <= waterdepth && waterdepth <= m_MaxDepth) && (steepness < m_MaxSlope) && (m_MinShore <= shoredist && shoredist <= m_MaxShore)); + } + + pass_class_t m_Mask; + + bool m_HasClearance; // whether static obstructions are impassable + fixed m_Clearance; // min distance from static obstructions + +private: + fixed m_MinDepth; + fixed m_MaxDepth; + fixed m_MaxSlope; + fixed m_MinShore; + fixed m_MaxShore; +}; + +#endif // INCLUDED_PATHFINDING diff --git a/source/simulation2/helpers/PriorityQueue.h b/source/simulation2/helpers/PriorityQueue.h index 9e7c8e37ee..475f073478 100644 --- a/source/simulation2/helpers/PriorityQueue.h +++ b/source/simulation2/helpers/PriorityQueue.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2010 Wildfire Games. +/* Copyright (C) 2015 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -32,14 +32,17 @@ template struct QueueItemPriority { - bool operator()(const Item& a, const Item& b) + bool operator()(const Item& a, const Item& b) const { if (CMP()(b.rank, a.rank)) // higher costs are lower priority return true; if (CMP()(a.rank, b.rank)) return false; // Need to tie-break to get a consistent ordering - // TODO: Should probably tie-break on g or h or something, but don't bother for now + if (CMP()(b.h, a.h)) // higher heuristic costs are lower priority + return true; + if (CMP()(a.h, b.h)) + return false; if (a.id < b.id) return true; if (b.id < a.id) @@ -57,7 +60,7 @@ struct QueueItemPriority * This is quite dreadfully slow in MSVC's debug STL implementation, * so we shouldn't use it unless we reimplement the heap functions more efficiently. */ -template > +template > class PriorityQueueHeap { public: @@ -65,6 +68,7 @@ public: { ID id; R rank; // f = g+h (estimated total cost of path through here) + H h; // heuristic cost }; void push(const Item& item) @@ -73,19 +77,10 @@ public: push_heap(m_Heap.begin(), m_Heap.end(), QueueItemPriority()); } - Item* find(ID id) + void promote(ID id, R UNUSED(oldrank), R newrank, H newh) { - for (size_t n = 0; n < m_Heap.size(); ++n) - { - if (m_Heap[n].id == id) - return &m_Heap[n]; - } - return NULL; - } - - void promote(ID id, R newrank) - { - for (size_t n = 0; n < m_Heap.size(); ++n) + // Loop backwards since it seems a little faster in practice + for (ssize_t n = m_Heap.size() - 1; n >= 0; --n) { if (m_Heap[n].id == id) { @@ -93,6 +88,7 @@ public: ENSURE(m_Heap[n].rank > newrank); #endif m_Heap[n].rank = newrank; + m_Heap[n].h = newh; push_heap(m_Heap.begin(), m_Heap.begin()+n+1, QueueItemPriority()); return; } @@ -104,7 +100,7 @@ public: #if PRIORITYQUEUE_DEBUG ENSURE(m_Heap.size()); #endif - Item r = m_Heap.front(); + Item r = m_Heap[0]; pop_heap(m_Heap.begin(), m_Heap.end(), QueueItemPriority()); m_Heap.pop_back(); return r; @@ -120,6 +116,11 @@ public: return m_Heap.size(); } + void clear() + { + m_Heap.clear(); + } + std::vector m_Heap; }; @@ -130,7 +131,7 @@ public: * It seems fractionally slower than a binary heap in optimised builds, but is * much simpler and less susceptible to MSVC's painfully slow debug STL. */ -template > +template > class PriorityQueueList { public: @@ -138,6 +139,7 @@ public: { ID id; R rank; // f = g+h (estimated total cost of path through here) + H h; // heuristic cost }; void push(const Item& item) @@ -155,9 +157,10 @@ public: return NULL; } - void promote(ID id, R newrank) + void promote(ID id, R UNUSED(oldrank), R newrank, H newh) { find(id)->rank = newrank; + find(id)->h = newh; } Item pop() @@ -193,6 +196,12 @@ public: return m_List.size(); } + + void clear() + { + m_List.clear(); + } + std::vector m_List; }; diff --git a/source/simulation2/helpers/Rasterize.cpp b/source/simulation2/helpers/Rasterize.cpp new file mode 100644 index 0000000000..d5660c05cf --- /dev/null +++ b/source/simulation2/helpers/Rasterize.cpp @@ -0,0 +1,107 @@ +/* Copyright (C) 2015 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 "Rasterize.h" + +#include "simulation2/helpers/Geometry.h" + +void SimRasterize::RasterizeRectWithClearance(Spans& spans, + const ICmpObstructionManager::ObstructionSquare& shape, + entity_pos_t clearance, entity_pos_t cellSize) +{ + // Get the bounds of cells that might possibly be within the shape + // (We'll then test each of those cells more precisely) + CFixedVector2D halfSize(shape.hw, shape.hh); + CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(shape.u, shape.v, halfSize); + // add 1 to at least have 1 tile out of reach + i16 iMax = (i16)( (halfBound.X + clearance) / cellSize).ToInt_RoundToInfinity() + 1; + i16 jMax = (i16)( (halfBound.Y + clearance) / cellSize).ToInt_RoundToInfinity() + 1; + + i16 offsetX = (i16)(shape.x / cellSize).ToInt_RoundToNearest(); + i16 offsetZ = (i16)(shape.z / cellSize).ToInt_RoundToNearest(); + + i16 i0 = -iMax; + i16 i1 = iMax; + + if (jMax <= 0) + return; // empty bounds - this shouldn't happen + + spans.reserve(jMax * 2); + + // TODO: Compare the squared distance to avoid sqrting +#define IS_IN_SQUARE(i, j) (Geometry::DistanceToSquare(CFixedVector2D(cellSize*i, cellSize*j), shape.u, shape.v, halfSize, true) <= clearance) + + // The rasterization is finished when for one row, all columns are visited and + // no tile in-range is found. + bool finished = false; + + // Loop over half of the rows + // Other rows can be added easily due to rectangle symmetry + // For each row, search the outer bounds, using the bounds of the previous row + // as an estimation + for (i16 j = 0; j <= jMax; ++j) + { + bool foundI0 = false; + // check if the estimation is in or out the square, and move accordingly + bool isI0InSquare = IS_IN_SQUARE(i0, j); + while (!foundI0 && !finished) + { + if (isI0InSquare && !IS_IN_SQUARE(--i0, j)) + { + foundI0 = true; + ++i0; // add one to bring i0 back in the square + } + else if (!isI0InSquare && IS_IN_SQUARE(++i0, j)) + foundI0 = true; + + // when this row has no obstructions, we're done + if (i0 > iMax) + finished = true; + ENSURE(i0 >= -iMax); + } + + if (finished) + break; + + bool foundI1 = false; + // check if the estimation is in or out the square, and move accordingly + bool isI1InSquare = IS_IN_SQUARE(i1, j); + while (!foundI1) + { + if (isI1InSquare && !IS_IN_SQUARE(++i1, j)) + { + foundI1 = true; + --i1; // subtract 1 to bring i1 back in the square + } + else if (!isI1InSquare && IS_IN_SQUARE(--i1, j)) + foundI1 = true; + // this row will have obstructions, or we will have stopped earlier + ENSURE(i1 >= i0 && i1 <= iMax); + } + + spans.emplace_back(Span{ (i16)(offsetX + i0), (i16)(offsetX + i1), (i16)(offsetZ + j) }); + // add symmetrical row from j == 1 onwards + if (j > 0) + spans.emplace_back(Span{ (i16)(offsetX - i1), (i16)(offsetX - i0), (i16)(offsetZ - j) }); + } + + // ensure that the entire bound was found + ENSURE(finished); +#undef IS_IN_SQUARE +} diff --git a/source/simulation2/helpers/Rasterize.h b/source/simulation2/helpers/Rasterize.h new file mode 100644 index 0000000000..bfee13ce8b --- /dev/null +++ b/source/simulation2/helpers/Rasterize.h @@ -0,0 +1,54 @@ +/* Copyright (C) 2012 Wildfire Games. + * This file is part of 0 A.D. + * + * 0 A.D. is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * 0 A.D. is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with 0 A.D. If not, see . + */ + +#ifndef INCLUDED_HELPER_RASTERIZE +#define INCLUDED_HELPER_RASTERIZE + +/** + * @file + * Helper functions related to rasterizing geometric shapes to grids. + */ + +#include "simulation2/components/ICmpObstructionManager.h" + +namespace SimRasterize +{ + +/** + * Represents the set of cells (i,j) where i0 <= i < i1 + */ +struct Span +{ + i16 i0; + i16 i1; + i16 j; +}; + +typedef std::vector Spans; + +/** + * Converts an ObstructionSquare @p shape (a rotated rectangle), + * expanded by the given @p clearance, + * into a list of spans of cells that are strictly inside the shape. + */ +void RasterizeRectWithClearance(Spans& spans, + const ICmpObstructionManager::ObstructionSquare& shape, + entity_pos_t clearance, entity_pos_t cellSize); + +} + +#endif // INCLUDED_HELPER_RASTERIZE diff --git a/source/simulation2/helpers/Render.cpp b/source/simulation2/helpers/Render.cpp index 2bee4d2fd7..07c2beb424 100644 --- a/source/simulation2/helpers/Render.cpp +++ b/source/simulation2/helpers/Render.cpp @@ -67,8 +67,11 @@ void SimRender::ConstructLineOnGround(const CSimContext& context, const std::vec } } -void SimRender::ConstructCircleOnGround(const CSimContext& context, float x, float z, float radius, - SOverlayLine& overlay, bool floating, float heightOffset) +static void ConstructCircleOrClosedArc( + const CSimContext& context, float x, float z, float radius, + bool isCircle, + float start, float end, + SOverlayLine& overlay, bool floating, float heightOffset) { overlay.m_Coords.clear(); @@ -85,20 +88,56 @@ void SimRender::ConstructCircleOnGround(const CSimContext& context, float x, flo } // Adapt the circle resolution to look reasonable for small and largeish radiuses - size_t numPoints = clamp((size_t)(radius*4.0f), (size_t)12, (size_t)48); + size_t numPoints = clamp((size_t)(radius*(end-start)), (size_t)12, (size_t)48); - overlay.m_Coords.reserve((numPoints + 1) * 3); + if (isCircle) + overlay.m_Coords.reserve((numPoints + 1 + 2) * 3); + else + overlay.m_Coords.reserve((numPoints + 1) * 3); + + float cy; + if (!isCircle) + { + // Start at the center point + cy = std::max(water, cmpTerrain->GetExactGroundLevel(x, z)) + heightOffset; + overlay.m_Coords.push_back(x); + overlay.m_Coords.push_back(cy); + overlay.m_Coords.push_back(z); + } for (size_t i = 0; i <= numPoints; ++i) // use '<=' so it's a closed loop { - float a = (float)i * 2 * (float)M_PI / (float)numPoints; - float px = x + radius * sinf(a); - float pz = z + radius * cosf(a); + float a = start + (float)i * (end - start) / (float)numPoints; + float px = x + radius * cosf(a); + float pz = z + radius * sinf(a); float py = std::max(water, cmpTerrain->GetExactGroundLevel(px, pz)) + heightOffset; overlay.m_Coords.push_back(px); overlay.m_Coords.push_back(py); overlay.m_Coords.push_back(pz); } + + if (!isCircle) + { + // Return to the center point + overlay.m_Coords.push_back(x); + overlay.m_Coords.push_back(cy); + overlay.m_Coords.push_back(z); + } +} + +void SimRender::ConstructCircleOnGround( + const CSimContext& context, float x, float z, float radius, + SOverlayLine& overlay, bool floating, float heightOffset) +{ + ConstructCircleOrClosedArc(context, x, z, radius, true, 0.0f, 2.0f*(float)M_PI, overlay, floating, heightOffset); +} + +void SimRender::ConstructClosedArcOnGround( + const CSimContext& context, float x, float z, float radius, + float start, float end, + SOverlayLine& overlay, bool floating, float heightOffset) +{ + ConstructCircleOrClosedArc(context, x, z, radius, false, start, end, overlay, floating, heightOffset); } // This method splits up a straight line into a number of line segments each having a length ~= TERRAIN_TILE_SIZE diff --git a/source/simulation2/helpers/Render.h b/source/simulation2/helpers/Render.h index 6ed8be28ec..fd4647d8a3 100644 --- a/source/simulation2/helpers/Render.h +++ b/source/simulation2/helpers/Render.h @@ -64,7 +64,8 @@ namespace SimRender * @param[in] floating If true, the line conforms to water as well. * @param[in] heightOffset Height above terrain to offset the line. */ -void ConstructLineOnGround(const CSimContext& context, const std::vector& xz, +void ConstructLineOnGround( + const CSimContext& context, const std::vector& xz, SOverlayLine& overlay, bool floating, float heightOffset = 0.25f); @@ -78,7 +79,18 @@ void ConstructLineOnGround(const CSimContext& context, const std::vector& * @param[in] heightOffset Height above terrain to offset the circle. * @param heightOffset The vertical offset to apply to points, to raise the line off the terrain a bit. */ -void ConstructCircleOnGround(const CSimContext& context, float x, float z, float radius, +void ConstructCircleOnGround( + const CSimContext& context, float x, float z, float radius, + SOverlayLine& overlay, + bool floating, float heightOffset = 0.25f); + +/** + * Constructs overlay line as an outlined circle sector (an arc with straight lines between the + * endpoints and the circle's center), conforming to terrain. + */ +void ConstructClosedArcOnGround( + const CSimContext& context, float x, float z, float radius, + float start, float end, SOverlayLine& overlay, bool floating, float heightOffset = 0.25f); @@ -92,7 +104,8 @@ void ConstructCircleOnGround(const CSimContext& context, float x, float z, float * @param[in] floating If true, the rectangle conforms to water as well. * @param[in] heightOffset Height above terrain to offset the rectangle. */ -void ConstructSquareOnGround(const CSimContext& context, float x, float z, float w, float h, float a, +void ConstructSquareOnGround( + const CSimContext& context, float x, float z, float w, float h, float a, SOverlayLine& overlay, bool floating, float heightOffset = 0.25f); diff --git a/source/simulation2/scripting/EngineScriptConversions.cpp b/source/simulation2/scripting/EngineScriptConversions.cpp index 63f7da7249..bfbeba69e8 100644 --- a/source/simulation2/scripting/EngineScriptConversions.cpp +++ b/source/simulation2/scripting/EngineScriptConversions.cpp @@ -290,3 +290,22 @@ template<> void ScriptInterface::ToJSVal >(JSContext* cx, JS::MutableH ret.setObject(*obj); } + +// TODO: This is copy-pasted from scriptinterface/ScriptConversions.h (#define VECTOR stuff), would be nice to remove the duplication +template<> void ScriptInterface::ToJSVal >(JSContext* cx, JS::MutableHandleValue ret, const std::vector& val) +{ + JSAutoRequest rq(cx); + JS::RootedObject obj(cx, JS_NewArrayObject(cx, 0)); + if (!obj) + { + ret.setUndefined(); + return; + } + for (size_t i = 0; i < val.size(); ++i) + { + JS::RootedValue el(cx); + ScriptInterface::ToJSVal(cx, &el, val[i]); + JS_SetElement(cx, obj, i, el); + } + ret.setObject(*obj); +} diff --git a/source/simulation2/serialization/SerializeTemplates.h b/source/simulation2/serialization/SerializeTemplates.h index 7cdb9e5fcd..0d54744b73 100644 --- a/source/simulation2/serialization/SerializeTemplates.h +++ b/source/simulation2/serialization/SerializeTemplates.h @@ -205,13 +205,13 @@ struct SerializeBool struct SerializeWaypoint { - void operator()(ISerializer& serialize, const char* UNUSED(name), const ICmpPathfinder::Waypoint& value) + void operator()(ISerializer& serialize, const char* UNUSED(name), const Waypoint& value) { serialize.NumberFixed_Unbounded("waypoint x", value.x); serialize.NumberFixed_Unbounded("waypoint z", value.z); } - void operator()(IDeserializer& deserialize, const char* UNUSED(name), ICmpPathfinder::Waypoint& value) + void operator()(IDeserializer& deserialize, const char* UNUSED(name), Waypoint& value) { deserialize.NumberFixed_Unbounded("waypoint x", value.x); deserialize.NumberFixed_Unbounded("waypoint z", value.z); @@ -221,9 +221,9 @@ struct SerializeWaypoint struct SerializeGoal { template - void operator()(S& serialize, const char* UNUSED(name), ICmpPathfinder::Goal& value) + void operator()(S& serialize, const char* UNUSED(name), PathGoal& value) { - SerializeU8_Enum()(serialize, "type", value.type); + SerializeU8_Enum()(serialize, "type", value.type); serialize.NumberFixed_Unbounded("goal x", value.x); serialize.NumberFixed_Unbounded("goal z", value.z); serialize.NumberFixed_Unbounded("goal u x", value.u.X); @@ -232,6 +232,7 @@ struct SerializeGoal serialize.NumberFixed_Unbounded("goal v z", value.v.Y); serialize.NumberFixed_Unbounded("goal hw", value.hw); serialize.NumberFixed_Unbounded("goal hh", value.hh); + serialize.NumberFixed_Unbounded("maxdist", value.maxdist); } }; diff --git a/source/tools/atlas/GameInterface/Handlers/TerrainHandlers.cpp b/source/tools/atlas/GameInterface/Handlers/TerrainHandlers.cpp index 6caa2a3240..b7c5b9dc5f 100644 --- a/source/tools/atlas/GameInterface/Handlers/TerrainHandlers.cpp +++ b/source/tools/atlas/GameInterface/Handlers/TerrainHandlers.cpp @@ -137,10 +137,10 @@ QUERYHANDLER(GetTerrainPassabilityClasses) CmpPtr cmpPathfinder(*AtlasView::GetView_Game()->GetSimulation2(), SYSTEM_ENTITY); if (cmpPathfinder) { - std::map classes = cmpPathfinder->GetPassabilityClasses(); + std::map classes = cmpPathfinder->GetPassabilityClasses(); std::vector classNames; - for (std::map::iterator it = classes.begin(); it != classes.end(); ++it) + for (std::map::iterator it = classes.begin(); it != classes.end(); ++it) classNames.push_back(CStr(it->first).FromUTF8()); msg->classNames = classNames; } diff --git a/source/tools/atlas/GameInterface/View.cpp b/source/tools/atlas/GameInterface/View.cpp index baa291bacf..ff996b3688 100644 --- a/source/tools/atlas/GameInterface/View.cpp +++ b/source/tools/atlas/GameInterface/View.cpp @@ -250,10 +250,9 @@ void AtlasViewGame::Render() { cmpPathfinder->SetDebugOverlay(true); // Kind of a hack to make it update the terrain grid - ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, fixed::Zero(), fixed::Zero() }; - ICmpPathfinder::pass_class_t passClass = cmpPathfinder->GetPassabilityClass(m_DisplayPassability); - ICmpPathfinder::cost_class_t costClass = cmpPathfinder->GetCostClass("default"); - cmpPathfinder->SetDebugPath(fixed::Zero(), fixed::Zero(), goal, passClass, costClass); + PathGoal goal = { PathGoal::POINT, fixed::Zero(), fixed::Zero() }; + pass_class_t passClass = cmpPathfinder->GetPassabilityClass(m_DisplayPassability); + cmpPathfinder->SetDebugPath(fixed::Zero(), fixed::Zero(), goal, passClass); } }