Index: source/simulation2/components/CCmpPathfinder.cpp =================================================================== --- source/simulation2/components/CCmpPathfinder.cpp +++ source/simulation2/components/CCmpPathfinder.cpp @@ -56,9 +56,9 @@ m_SameTurnMovesCount = 0; - m_VertexPathfinder = std::unique_ptr(new VertexPathfinder(m_MapSize, m_TerrainOnlyGrid)); m_LongPathfinder = std::unique_ptr(new LongPathfinder()); m_PathfinderHier = std::unique_ptr(new HierarchicalPathfinder()); + m_VertexPathfinder = std::unique_ptr(new VertexPathfinder(m_MapSize, m_TerrainOnlyGrid, m_Grid, *m_PathfinderHier)); // Register Relax NG validator CXeromyces::AddValidator(g_VFS, "pathfinder", "simulation/data/pathfinder.rng"); Index: source/simulation2/components/ICmpPathfinder.h =================================================================== --- source/simulation2/components/ICmpPathfinder.h +++ source/simulation2/components/ICmpPathfinder.h @@ -33,6 +33,30 @@ template class Grid; +struct AsyncLongPathRequest +{ + u32 ticket; + entity_pos_t x0; + entity_pos_t z0; + PathGoal goal; + pass_class_t passClass; + entity_id_t notify; +}; + +struct AsyncShortPathRequest +{ + u32 ticket; + entity_pos_t x0; + entity_pos_t z0; + entity_pos_t clearance; + entity_pos_t range; + PathGoal goal; + pass_class_t passClass; + bool avoidMovingUnits; + entity_id_t group; + entity_id_t notify; +}; + /** * Pathfinder algorithms. * Index: source/simulation2/helpers/HierarchicalPathfinder.h =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.h +++ source/simulation2/helpers/HierarchicalPathfinder.h @@ -19,6 +19,7 @@ #define INCLUDED_HIERPATHFINDER #include "Pathfinding.h" +#include "PathGoal.h" #include "renderer/TerrainOverlay.h" #include "Render.h" Index: source/simulation2/helpers/LongPathfinder.h =================================================================== --- source/simulation2/helpers/LongPathfinder.h +++ source/simulation2/helpers/LongPathfinder.h @@ -19,8 +19,9 @@ #define INCLUDED_LONGPATHFINDER #include "Pathfinding.h" - +#include "PathGoal.h" #include "PriorityQueue.h" + #include "graphics/Overlay.h" #include "renderer/Scene.h" #include "renderer/TerrainOverlay.h" Index: source/simulation2/helpers/PathGoal.h =================================================================== --- source/simulation2/helpers/PathGoal.h +++ source/simulation2/helpers/PathGoal.h @@ -19,6 +19,7 @@ #define INCLUDED_PATHGOAL #include "maths/FixedVector2D.h" +#include "simulation2/helpers/Pathfinding.h" #include "simulation2/helpers/Position.h" /** @@ -79,6 +80,11 @@ * Returns the coordinates of the point on the goal that is closest to the point pos. */ CFixedVector2D NearestPointOnGoal(CFixedVector2D pos) const; + + /** + * Return a vector of Points that are in the goal and on passable navcells. + */ + std::vector ReturnPassableSubGoals(const Grid* grid, pass_class_t passClass) const; }; #endif // INCLUDED_PATHGOAL Index: source/simulation2/helpers/PathGoal.cpp =================================================================== --- source/simulation2/helpers/PathGoal.cpp +++ source/simulation2/helpers/PathGoal.cpp @@ -294,3 +294,63 @@ NODEFAULT; } } + +std::vector PathGoal::ReturnPassableSubGoals(const Grid* grid, pass_class_t passClass) const +{ + CFixedVector2D g(x, z); + u16 gi, gj; + Pathfinding::NearestNavcell(x, z, gi, gj, grid->m_W, grid->m_H); + + CFixedVector2D u(fixed::FromInt(1), fixed::FromInt(0)), v(fixed::FromInt(0), fixed::FromInt(1)); + CFixedVector2D size(fixed::FromInt(1) - fixed::Epsilon(), fixed::FromInt(1) - fixed::Epsilon()); + + std::vector subGoals; + if (type == POINT) + // don't check if it's actually passable, if it's not we can't return a reasonable result anyways. + return { g }; + + if (type == CIRCLE || type == INVERTED_CIRCLE) + { + u16 goalRadiusSquared = hw.ToInt_RoundToInfinity() * hw.ToInt_RoundToInfinity(); + for (u16 j = gj - hw.ToInt_RoundToInfinity(); j <= gj + hw.ToInt_RoundToInfinity(); ++j) + for (u16 i = gi - hw.ToInt_RoundToInfinity(); i <= gi + hw.ToInt_RoundToInfinity(); ++i) + { + // Skip navcells not on the circle's edge. + if (abs((i - gi) * (i - gi) + (j - gj) * (j - gj) - goalRadiusSquared) > Pathfinding::NAVCELL_SIZE_INT) + continue; + + if (!NavcellContainsGoal(i, j)) + continue; + + if (!IS_PASSABLE(grid->get(i, j), passClass)) + continue; + + CFixedVector2D out(fixed::FromInt(i - gi), fixed::FromInt(j - gj)); + Geometry::NearestPointOnSquare(out, u, v, size); + subGoals.push_back(out); + } + } else { + u16 maxRadius = (hw.Square() + hh.Square()).ToInt_RoundToInfinity(); + u16 minRadius = std::min(hw, hh).ToInt_RoundToZero() * std::min(hw, hh).ToInt_RoundToZero(); + for (u16 j = gj - hh.ToInt_RoundToInfinity(); j <= gj + hh.ToInt_RoundToInfinity(); ++j) + for (u16 i = gi - hw.ToInt_RoundToInfinity(); i <= gi + hw.ToInt_RoundToInfinity(); ++i) + { + // Skip navcells that are outside the outer-bounds circle or inside the inner-bounds circle. + if ((i - gi) * (i - gi) + (j - gj) * (j - gj) > maxRadius || (i - gi) * (i - gi) + (j - gj) * (j - gj) < minRadius) + continue; + + if (!NavcellContainsGoal(i, j)) + continue; + + if (!IS_PASSABLE(grid->get(i, j), passClass)) + continue; + + CFixedVector2D out(fixed::FromInt(i - gi), fixed::FromInt(j - gj)); + Geometry::NearestPointOnSquare(out, u, v, size); + out.X = out.X + fixed::FromInt(gi); + out.Y = out.Y + fixed::FromInt(gj); + subGoals.push_back(out); + } + } + return subGoals; +} Index: source/simulation2/helpers/Pathfinding.h =================================================================== --- source/simulation2/helpers/Pathfinding.h +++ source/simulation2/helpers/Pathfinding.h @@ -19,40 +19,17 @@ #define INCLUDED_PATHFINDING #include "maths/MathUtil.h" +#include "maths/FixedVector2D.h" #include "ps/CLogger.h" #include "simulation2/system/Entity.h" #include "simulation2/system/ParamNode.h" #include "graphics/Terrain.h" #include "Grid.h" -#include "PathGoal.h" +#include "Position.h" typedef u16 pass_class_t; -struct AsyncLongPathRequest -{ - u32 ticket; - entity_pos_t x0; - entity_pos_t z0; - PathGoal goal; - pass_class_t passClass; - entity_id_t notify; -}; - -struct AsyncShortPathRequest -{ - u32 ticket; - entity_pos_t x0; - entity_pos_t z0; - entity_pos_t clearance; - entity_pos_t range; - PathGoal goal; - pass_class_t passClass; - bool avoidMovingUnits; - entity_id_t group; - entity_id_t notify; -}; - struct Waypoint { entity_pos_t x, z; Index: source/simulation2/helpers/VertexPathfinder.h =================================================================== --- source/simulation2/helpers/VertexPathfinder.h +++ source/simulation2/helpers/VertexPathfinder.h @@ -22,6 +22,9 @@ #include "simulation2/helpers/Pathfinding.h" #include "simulation2/system/CmpPtr.h" +struct AsyncShortPathRequest; +class PathGoal; + // A vertex around the corners of an obstruction // (paths will be sequences of these vertexes) struct Vertex @@ -67,12 +70,14 @@ class ICmpObstructionManager; class CSimContext; +class HierarchicalPathfinder; class SceneCollector; class VertexPathfinder { public: - VertexPathfinder(const u16& mapSize, Grid* const & terrainOnlyGrid) : m_MapSize(mapSize), m_TerrainOnlyGrid(terrainOnlyGrid), m_DebugOverlay(false) {}; + VertexPathfinder(const u16& mapSize, Grid* const & terrainOnlyGrid, Grid* const & grid, const HierarchicalPathfinder& hp) + : m_MapSize(mapSize), m_TerrainOnlyGrid(terrainOnlyGrid), m_Grid(grid), m_HierarchicalPathfinder(hp), m_DebugOverlay(false) {}; /** * Compute a precise path from the given point to the goal, and return the set of waypoints. @@ -95,6 +100,8 @@ // References to the Pathfinder for convenience. const u16& m_MapSize; Grid* const & m_TerrainOnlyGrid; + Grid* const & m_Grid; + const HierarchicalPathfinder& m_HierarchicalPathfinder; std::atomic m_DebugOverlay; mutable std::vector m_DebugOverlayShortPathLines; Index: source/simulation2/helpers/VertexPathfinder.cpp =================================================================== --- source/simulation2/helpers/VertexPathfinder.cpp +++ source/simulation2/helpers/VertexPathfinder.cpp @@ -38,6 +38,9 @@ #include "ps/Profile.h" #include "renderer/Scene.h" #include "simulation2/components/ICmpObstructionManager.h" +#include "simulation2/components/ICmpPathfinder.h" +#include "simulation2/helpers/HierarchicalPathfinder.h" +#include "simulation2/helpers/PathGoal.h" #include "simulation2/helpers/PriorityQueue.h" #include "simulation2/helpers/Render.h" #include "simulation2/system/SimContext.h" @@ -630,26 +633,39 @@ AddTerrainEdges(m_Edges, m_Vertexes, i0, j0, i1, j1, request.passClass, *m_TerrainOnlyGrid); } - // Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable) - for (size_t i = 0; i < m_EdgeSquares.size(); ++i) + u16 i, j; + Pathfinding::NearestNavcell(start.p.X, start.p.Y, i, j, m_Grid->m_W, m_Grid->m_H); + HierarchicalPathfinder::GlobalRegionID startRegion = m_HierarchicalPathfinder.GetGlobalRegion(i, j, request.passClass); + + // Remove some trivially unreachable vertices by closing them (remove() would be too slow) + for (size_t k = 2; k < m_Vertexes.size(); ++k) { - // If the start point is inside the square, ignore it - if (start.p.X >= m_EdgeSquares[i].p0.X && - start.p.Y >= m_EdgeSquares[i].p0.Y && - start.p.X <= m_EdgeSquares[i].p1.X && - start.p.Y <= m_EdgeSquares[i].p1.Y) + // Remove vertices that are on navcells of another global region. + Pathfinding::NearestNavcell(m_Vertexes[k].p.X, m_Vertexes[k].p.Y, i, j, m_Grid->m_W, m_Grid->m_H); + if (startRegion != 0 && startRegion != m_HierarchicalPathfinder.GetGlobalRegion(i, j, request.passClass)) + { + m_Vertexes[k].status = Vertex::CLOSED; continue; + } - // Remove every non-start/goal vertex that is inside an edgeSquare; - // since remove() would be inefficient, just mark it as closed instead. - for (size_t j = 2; j < m_Vertexes.size(); ++j) - if (m_Vertexes[j].p.X >= m_EdgeSquares[i].p0.X && - m_Vertexes[j].p.Y >= m_EdgeSquares[i].p0.Y && - m_Vertexes[j].p.X <= m_EdgeSquares[i].p1.X && - m_Vertexes[j].p.Y <= m_EdgeSquares[i].p1.Y) - m_Vertexes[j].status = Vertex::CLOSED; + // Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable) + for (size_t l = 0; l < m_EdgeSquares.size(); ++l) + { + // If the start point is inside the square, ignore it + if (start.p.X >= m_EdgeSquares[l].p0.X && + start.p.Y >= m_EdgeSquares[l].p0.Y && + start.p.X <= m_EdgeSquares[l].p1.X && + start.p.Y <= m_EdgeSquares[l].p1.Y) + continue; + if (m_Vertexes[k].p.X >= m_EdgeSquares[l].p0.X && + m_Vertexes[k].p.Y >= m_EdgeSquares[l].p0.Y && + m_Vertexes[k].p.X <= m_EdgeSquares[l].p1.X && + m_Vertexes[k].p.Y <= m_EdgeSquares[l].p1.Y) + m_Vertexes[k].status = Vertex::CLOSED; + } } + ENSURE(m_Vertexes.size() < 65536); // We store array indexes as u16. DebugRenderGraph(cmpObstructionManager->GetSimContext(), m_Vertexes, m_Edges, m_EdgeSquares); @@ -672,6 +688,8 @@ u16 idBest = START_VERTEX_ID; fixed hBest = start.h; + std::vector subGoals; + while (!open.empty()) { // Move best tile from open to closed @@ -711,6 +729,43 @@ { npos = request.goal.NearestPointOnGoal(m_Vertexes[curr.id].p); + // If this point is on an impassable navcell, replace it with the nearest reachable point. + u16 i, j; + Pathfinding::NearestNavcell(npos.X, npos.Y, i, j, m_Grid->m_W, m_Grid->m_H); + if (!IS_PASSABLE(m_Grid->get(i, j), request.passClass)) + { + if (subGoals.empty()) + { + subGoals = request.goal.ReturnPassableSubGoals(m_Grid, request.passClass); + subGoals.erase(std::remove_if(subGoals.begin(), subGoals.end(), [this, &startRegion, &request](CFixedVector2D pos) { + u16 i, j; + Pathfinding::NearestNavcell(pos.X, pos.Y, i, j, m_Grid->m_W, m_Grid->m_H); + if (startRegion != 0 && startRegion != m_HierarchicalPathfinder.GetGlobalRegion(i, j, request.passClass)) + return true; + return false; + }), subGoals.end()); + } + + // if there are still no sub-goals, our goal is not reachable, so return early. + if (subGoals.empty()) + { + // Exit the loop with no results. + m_Vertexes[n].status = Vertex::CLOSED; + open.clear(); + break; + } + size_t i = 0; + CFixedVector2D vec2Best = CFixedVector2D(subGoals[i].X - m_Vertexes[curr.id].p.X, subGoals[i].Y - m_Vertexes[curr.id].p.Y); + for (;i < subGoals.size(); ++i) + { + CFixedVector2D vec = CFixedVector2D(subGoals[i].X - m_Vertexes[curr.id].p.X, subGoals[i].Y - m_Vertexes[curr.id].p.Y); + if (vec.CompareLength(vec2Best) < 0) + vec2Best = vec; + } + npos.X = vec2Best.X + m_Vertexes[curr.id].p.X; + npos.Y = vec2Best.Y + m_Vertexes[curr.id].p.Y; + } + // To prevent integer overflows later on, we need to ensure all vertexes are // 'close' to the source. The goal might be far away (not a good idea but // sometimes it happens), so clamp it to the current search range