Ticket #1756: perfpath6-quantumstate.diff

source/tools/atlas/GameInterface/View.cpp

@@ -254 +254 @@
         {
             cmpPathfinder->SetDebugOverlay(true);
             // Kind of a hack to make it update the terrain grid
-            ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, fixed::Zero(), fixed::Zero() };
+            PathGoal goal = { PathGoal::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);
+            cmpPathfinder->SetDebugPath(fixed::Zero(), fixed::Zero(), goal, passClass);
         }
     }
 

source/simulation2/serialization/SerializeTemplates.h

@@ -191 +191 @@
 struct SerializeGoal
 {
     template<typename S>
-    void operator()(S& serialize, const char* UNUSED(name), ICmpPathfinder::Goal& value)
+    void operator()(S& serialize, const char* UNUSED(name), PathGoal& value)
     {
-        SerializeU8_Enum<ICmpPathfinder::Goal::Type, ICmpPathfinder::Goal::SQUARE>()(serialize, "type", value.type);
+        SerializeU8_Enum<PathGoal::Type, PathGoal::SQUARE>()(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);

source/simulation2/helpers/Render.h

@@ -64 +64 @@
  * @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<float>& xz,
+void ConstructLineOnGround(
+        const CSimContext& context, const std::vector<float>& xz,
         SOverlayLine& overlay,
         bool floating, float heightOffset = 0.25f);
 
@@ -78 +79 @@
  * @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);
+
+/**
  * Constructs overlay line as rectangle with given center and dimensions, conforming to terrain.
  *
  * @param[in] x,z Coordinates of center of rectangle.
@@ -92 +104 @@
  * @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);
 

source/simulation2/helpers/Render.cpp

@@ -67 +67 @@
     }
 }
 
-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();
 
@@ -79 +82 @@
     float water = 0.f;
     if (floating)
     {
-        CmpPtr<ICmpWaterManager> cmpWaterManager(context, SYSTEM_ENTITY);
-        if (cmpWaterManager)
-            water = cmpWaterManager->GetExactWaterLevel(x, z);
+        CmpPtr<ICmpWaterManager> cmpWaterMan(context, SYSTEM_ENTITY);
+        if (cmpWaterMan)
+            water = cmpWaterMan->GetExactWaterLevel(x, z);
     }
 
     // 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
 static void SplitLine(std::vector<std::pair<float, float> >& coords, float x1, float y1, float x2, float y2)
 {

source/simulation2/helpers/Rasterize.h

@@ +1 @@
+/* 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 <http://www.gnu.org/licenses/>.
+ */
+
+#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<Span> 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

source/simulation2/helpers/Rasterize.cpp

@@ +1 @@
+/* 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 <http://www.gnu.org/licenses/>.
+ */
+
+#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 + clearance, shape.hh + clearance);
+    CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(shape.u, shape.v, halfSize);
+    i16 i0 = ((shape.x - halfBound.X) / cellSize).ToInt_RoundToNegInfinity();
+    i16 j0 = ((shape.z - halfBound.Y) / cellSize).ToInt_RoundToNegInfinity();
+    i16 i1 = ((shape.x + halfBound.X) / cellSize).ToInt_RoundToInfinity();
+    i16 j1 = ((shape.z + halfBound.Y) / cellSize).ToInt_RoundToInfinity();
+
+    if (j1 <= j0)
+        return; // empty bounds - this shouldn't happen
+
+    spans.reserve(j1 - j0);
+
+    for (i16 j = j0; j < j1; ++j)
+    {
+        // Find the min/max range of cells that are strictly inside the square+clearance.
+        // (Since the square+clearance is a convex shape, we can just test each
+        // corner of each cell is inside the shape.)
+        // (TODO: This potentially does a lot of redundant work.)
+        i16 spanI0 = std::numeric_limits<i16>::max();
+        i16 spanI1 = std::numeric_limits<i16>::min();
+        for (i16 i = i0; i < i1; ++i)
+        {
+            if (Geometry::DistanceToSquare(
+                CFixedVector2D(cellSize*i, cellSize*j) - CFixedVector2D(shape.x, shape.z),
+                shape.u, shape.v, CFixedVector2D(shape.hw, shape.hh), true) > clearance)
+            {
+                continue;
+            }
+
+            if (Geometry::DistanceToSquare(
+                CFixedVector2D(cellSize*(i+1), cellSize*j) - CFixedVector2D(shape.x, shape.z),
+                shape.u, shape.v, CFixedVector2D(shape.hw, shape.hh), true) > clearance)
+            {
+                continue;
+            }
+
+            if (Geometry::DistanceToSquare(
+                CFixedVector2D(cellSize*i, cellSize*(j+1)) - CFixedVector2D(shape.x, shape.z),
+                shape.u, shape.v, CFixedVector2D(shape.hw, shape.hh), true) > clearance)
+            {
+                continue;
+            }
+
+            if (Geometry::DistanceToSquare(
+                CFixedVector2D(cellSize*(i+1), cellSize*(j+1)) - CFixedVector2D(shape.x, shape.z),
+                shape.u, shape.v, CFixedVector2D(shape.hw, shape.hh), true) > clearance)
+            {
+                continue;
+            }
+
+            spanI0 = std::min(spanI0, i);
+            spanI1 = std::max(spanI1, (i16)(i+1));
+        }
+
+        // Add non-empty spans onto the list
+        if (spanI0 < spanI1)
+        {
+            Span span = { spanI0, spanI1, j };
+            spans.push_back(span);
+        }
+    }
+}

source/simulation2/helpers/PriorityQueue.h

@@ -32 +32 @@
 template <typename Item, typename CMP>
 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 (a.id < b.id)
             return true;
         if (b.id < a.id)
@@ -73 +72 @@
         push_heap(m_Heap.begin(), m_Heap.end(), QueueItemPriority<Item, CMP>());
     }
 
-    Item* find(ID id)
+    void promote(ID id, R UNUSED(oldrank), 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)
-                return &m_Heap[n];
-        }
-        return NULL;
-    }
-
-    void promote(ID id, R newrank)
-    {
-        for (size_t n = 0; n < m_Heap.size(); ++n)
-        {
-            if (m_Heap[n].id == id)
             {
 #if PRIORITYQUEUE_DEBUG
                 ENSURE(m_Heap[n].rank > newrank);
@@ -104 +94 @@
 #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<Item, CMP>());
         m_Heap.pop_back();
         return r;
@@ -155 +145 @@
         return NULL;
     }
 
-    void promote(ID id, R newrank)
+    void promote(ID id, R UNUSED(oldrank), R newrank)
     {
         find(id)->rank = newrank;
     }

source/simulation2/helpers/PathGoal.h

@@ +1 @@
+/* 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 <http://www.gnu.org/licenses/>.
+ */
+
+#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
+
+    /**
+     * 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;
+};
+
+#endif // INCLUDED_PATHGOAL

source/simulation2/helpers/PathGoal.cpp

@@ +1 @@
+/* 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "precompiled.h"
+
+#include "PathGoal.h"
+
+#include "graphics/Terrain.h"
+#include "simulation2/components/ICmpObstructionManager.h"
+#include "simulation2/helpers/Geometry.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(ICmpObstructionManager::NAVCELL_SIZE);
+    entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+    entity_pos_t x1 = x0 + ICmpObstructionManager::NAVCELL_SIZE;
+    entity_pos_t z1 = z0 + ICmpObstructionManager::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(ICmpObstructionManager::NAVCELL_SIZE);
+    entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+    entity_pos_t x1 = x0 + ICmpObstructionManager::NAVCELL_SIZE;
+    entity_pos_t z1 = z0 + ICmpObstructionManager::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 / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToZero();
+        int gj = (z / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToZero();
+        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);
+    default:
+    {
+        debug_warn(L"invalid type");
+        return false;
+    }
+    }
+}
+
+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 / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToZero();
+        int j = (z / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToZero();
+        // 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(ICmpObstructionManager::NAVCELL_SIZE);
+                entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+                entity_pos_t x1 = x0 + ICmpObstructionManager::NAVCELL_SIZE;
+                entity_pos_t z1 = z0 + ICmpObstructionManager::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(ICmpObstructionManager::NAVCELL_SIZE);
+                entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+                entity_pos_t x1 = x0 + ICmpObstructionManager::NAVCELL_SIZE;
+                entity_pos_t z1 = z0 + ICmpObstructionManager::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;
+        }
+    default:
+    {
+        debug_warn(L"invalid type");
+        return false;
+    }
+    }
+}
+
+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;
+    }
+    default:
+    {
+        debug_warn(L"invalid type");
+        return false;
+    }
+    }
+}
+
+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);
+    }
+
+    default:
+        debug_warn(L"invalid type");
+        return fixed::Zero();
+    }
+}

source/simulation2/helpers/Grid.h

@@ -48 +48 @@
 
     Grid(const Grid& g)
     {
+        m_Data = NULL;
         *this = g;
     }
 
@@ -58 +59 @@
             m_W = g.m_W;
             m_H = g.m_H;
             m_DirtyID = g.m_DirtyID;
+            delete[] m_Data;
             if (g.m_Data)
             {
                 m_Data = new T[m_W * m_H];

source/simulation2/helpers/Geometry.h

@@ -32 +32 @@
 {
 
 /**
- * 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);
+bool PointIsInSquare(CFixedVector2D point,
+    CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize);
 
+/**
+ * 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);
 
-fixed DistanceToSquare(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.
+ * 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.
  */
-float ChordToCentralAngle(const float chordLength, const float radius);
+fixed DistanceToSquare(CFixedVector2D point,
+    CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize,
+    bool countInsideAsZero = false);
 
 /**
- * Find point closest to the given point on the edge of the given square or rectangle.
+ * Returns a point on the boundary of the given rotated rectangle
+ * that is closest (or equally closest) to the given point
+ * in Euclidean distance.
  *
- * @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.
+ * The rectangle is defined by the four vertexes
+ * (+/-u*halfSize.X +/-v*halfSize.Y).
  *
- * @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
+ * The @p u and @p v vectors must be perpendicular and unit length.
  */
-CFixedVector2D NearestPointOnSquare(CFixedVector2D point, CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize);
+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);
+
 bool TestRaySquare(CFixedVector2D a, CFixedVector2D b, CFixedVector2D u, CFixedVector2D v, CFixedVector2D halfSize);
 
 bool TestRayAASquare(CFixedVector2D a, CFixedVector2D b, CFixedVector2D halfSize);

source/simulation2/helpers/Geometry.cpp

@@ -50 +50 @@
     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 +92 @@
         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
+        {
+            if (countInsideAsZero)
+                closest = fixed::Zero();
+            else
+                closest = std::min(closest, (du.Absolute() - hw).Absolute()); // vertical edges
+        }
 
         return closest;
     }

source/simulation2/components/tests/test_Pathfinder.h

@@ -26 +26 @@
 #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
@@ -85 +86 @@
         ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, x1, z1 };
 
         ICmpPathfinder::Path path;
-        cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), cmp->GetCostClass("default"), 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 +100 @@
             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);
+            cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), path);
         }
 
         t = timer_Time() - t;
@@ -133 +134 @@
         }
 
         NullObstructionFilter filter;
-        ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, range, range };
+        PathGoal goal = { PathGoal::POINT, range, range };
         ICmpPathfinder::Path 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<typename T>
+    void DumpGrid(std::ostream& stream, const Grid<T>& 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 << "  <rect x='" << i0 << "' y='" << j << "' width='" << (i-i0) << "' height='1'/>\n";
+                        break;
+                    }
+                }
+            }
+        }
+    }
+
+#define USE_JPS 1
+
+    void test_perf2()
+    {
+        CTerrain terrain;
+
+        CSimulation2 sim2(NULL, &terrain);
+        sim2.LoadDefaultScripts();
+        sim2.ResetState();
+
+        CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself
+
+        LDR_BeginRegistering();
+        mapReader->LoadMap(L"maps/scenarios/Peloponnese.pmp", &terrain, 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<ICmpObstructionManager> cmpObstructionManager(sim2, SYSTEM_ENTITY);
+        CmpPtr<ICmpPathfinder> cmpPathfinder(sim2, SYSTEM_ENTITY);
+
+        ICmpPathfinder::pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default") | ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING;
+        const Grid<u16>& obstructions = cmpPathfinder->GetPassabilityGrid();
+
+        int scale = 1;
+        stream << "<!DOCTYPE html>\n";
+        stream << "<style>\n";
+        stream << ".passability rect { fill: black; }\n";
+        stream << ".astar-open rect { fill: rgba(255,255,0,0.75); }\n";
+        stream << ".astar-closed rect { fill: rgba(255,0,0,0.75); }\n";
+//      stream << ".astar-closed rect { fill: rgba(0,255,0,0.75); }\n";
+        stream << ".path { stroke: rgba(0,0,255,0.75); stroke-width: 1; fill: none; }\n";
+        stream << "</style>\n";
+        stream << "<svg width='" << obstructions.m_W*scale << "' height='" << obstructions.m_H*scale << "'>\n";
+        stream << "<g transform='translate(0 " << obstructions.m_H*scale << ") scale(" << scale << " -" << scale << ")'>\n";
+
+        stream << " <g class='passability'>\n";
+        DumpGrid(stream, obstructions, obstructionsMask);
+        stream << " </g>\n";
+
+        DumpPath(stream, 128*4, 256*4, 128*4, 384*4, cmpPathfinder);
+//      RepeatPath(500, 128*4, 256*4, 128*4, 384*4, cmpPathfinder);
+// //       RepeatPath(100, 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 << "</g>\n";
+        stream << "</svg>\n";
+    }
+    
+    void test_perf3()
+    {
+        CTerrain terrain;
+
+        CSimulation2 sim2(NULL, &terrain);
+        sim2.LoadDefaultScripts();
+        sim2.ResetState();
+
+        CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself
+
+        LDR_BeginRegistering();
+        mapReader->LoadMap(L"maps/scenarios/pathfindtest7.pmp", &terrain, 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<ICmpObstructionManager> cmpObstructionManager(sim2, SYSTEM_ENTITY);
+        CmpPtr<ICmpPathfinder> cmpPathfinder(sim2, SYSTEM_ENTITY);
+
+        ICmpPathfinder::pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default") | ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING;
+        const Grid<u16>& obstructions = cmpPathfinder->GetPassabilityGrid();
+
+        int scale = 31;
+        stream << "<!DOCTYPE html>\n";
+        stream << "<style>\n";
+        stream << ".passability rect { fill: black; }\n";
+        stream << ".astar-open rect { fill: rgba(255,255,0,0.75); }\n";
+        stream << ".astar-closed rect { fill: rgba(255,0,0,0.75); }\n";
+        stream << ".path { stroke: rgba(0,0,255,0.75); stroke-width: " << (1.0f / scale) << "; fill: none; }\n";
+        stream << "</style>\n";
+        stream << "<svg width='" << obstructions.m_W*scale << "' height='" << obstructions.m_H*scale << "'>\n";
+        stream << "<defs><pattern id='grid' width='1' height='1' patternUnits='userSpaceOnUse'>\n";
+        stream << "<rect fill='none' stroke='#888' stroke-width='" << (1.0f / scale) << "' width='" << scale << "' height='" << scale << "'/>\n";
+        stream << "</pattern></defs>\n";
+        stream << "<g transform='translate(0 " << obstructions.m_H*scale << ") scale(" << scale << " -" << scale << ")'>\n";
+
+        stream << " <g class='passability'>\n";
+        DumpGrid(stream, obstructions, obstructionsMask);
+        stream << " </g>\n";
+
+        for (int j = 160; j < 190; ++j)
+            for (int i = 220; i < 290; ++i)
+                DumpPath(stream, 230, 178, i, j, cmpPathfinder);
+
+        stream << "<rect fill='url(#grid)' width='100%' height='100%'/>\n";
+        stream << "</g>\n";
+        stream << "</svg>\n";
+    }
+
+    void DumpPath(std::ostream& stream, int i0, int j0, int i1, int j1, CmpPtr<ICmpPathfinder>& 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 };
+
+        ICmpPathfinder::Path path;
+#if USE_JPS
+        cmpPathfinder->ComputePathJPS(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path);
+#else
+        cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path);
+#endif
+
+        u32 debugSteps;
+        double debugTime;
+        Grid<u8> debugGrid;
+#if USE_JPS
+        cmpPathfinder->GetDebugDataJPS(debugSteps, debugTime, debugGrid);
+#else
+        cmpPathfinder->GetDebugData(debugSteps, debugTime, debugGrid);
+#endif
+//      stream << " <g style='visibility:hidden'>\n";
+
+        stream << " <g>\n";
+//      stream << " <g class='astar-open'>\n";
+//      DumpGrid(stream, debugGrid, 1);
+//      stream << " </g>\n";
+//      stream << " <g class='astar-closed'>\n";
+//      DumpGrid(stream, debugGrid, 2);
+//      stream << " </g>\n";
+//      stream << " <g class='astar-closed'>\n";
+//      DumpGrid(stream, debugGrid, 3);
+//      stream << " </g>\n";
+        stream << " </g>\n";
+
+        stream << " <polyline";
+        stream << " onmouseover='this.previousElementSibling.style.visibility=\"visible\"' onmouseout='this.previousElementSibling.style.visibility=\"hidden\"'";
+        double length = 0;
+        for (ssize_t i = 0; i < (ssize_t)path.m_Waypoints.size()-1; ++i)
+        {
+            double dx = (path.m_Waypoints[i+1].x - path.m_Waypoints[i].x).ToDouble();
+            double dz = (path.m_Waypoints[i+1].z - path.m_Waypoints[i].z).ToDouble();
+            ENSURE(dx == 0 || dz == 0 || abs(dx) == abs(dz));
+            length += sqrt(dx*dx + dz*dz);
+        }
+        stream << " title='length: " << length << "; tiles explored: " << debugSteps << "; time: " << debugTime*1000 << " msec'";
+        stream << " class='path' points='";
+        for (size_t i = 0; i < path.m_Waypoints.size(); ++i)
+            stream << path.m_Waypoints[i].x.ToDouble()*ICmpObstructionManager::NAVCELLS_PER_TILE/TERRAIN_TILE_SIZE << "," << path.m_Waypoints[i].z.ToDouble()*ICmpObstructionManager::NAVCELLS_PER_TILE/TERRAIN_TILE_SIZE << " ";
+        stream << "'/>\n";
+    }
+
+    void RepeatPath(int n, int i0, int j0, int i1, int j1, CmpPtr<ICmpPathfinder>& 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)
+        {
+            ICmpPathfinder::Path path;
+#if USE_JPS
+            cmpPathfinder->ComputePathJPS(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path);
+#else
+            cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path);
+#endif
+        }
+        t = timer_Time() - t;
+        debug_printf(L"### RepeatPath %fms each (%fs total)\n", 1000*t / n, t);
+    }
+
 };

source/simulation2/components/ICmpUnitMotion.h

@@ -1 @@
-/* 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
@@ -38 +38 @@
 
     /**
      * 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 +61 @@
 
     /**
      * 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,

source/simulation2/components/ICmpTerritoryManager.h

@@ -29 +29 @@
 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 = 0x3F;
     static const int TERRITORY_CONNECTED_MASK = 0x40;
     static const int TERRITORY_PROCESSED_MASK = 0x80; //< For internal use; marks a tile as processed.

source/simulation2/components/ICmpPathfinder.h

@@ -1 @@
-/* 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
@@ -20 +20 @@
 
 #include "simulation2/system/Interface.h"
 
+#include "simulation2/helpers/PathGoal.h"
 #include "simulation2/helpers/Position.h"
 
 #include "maths/FixedVector2D.h"
@@ -38 +39 @@
  * 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 +49 @@
 {
 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;
@@ -87 +75 @@
      */
     virtual pass_class_t GetPassabilityClass(const std::string& name) = 0;
 
-    /**
-     * Get the tag for a given movement cost class name.
-     * Logs an error and returns something acceptable if the name is unrecognised.
-     */
-    virtual cost_class_t GetCostClass(const std::string& name) = 0;
-
     virtual const Grid<u16>& GetPassabilityGrid() = 0;
 
     /**
@@ -100 +82 @@
      * 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, Path& ret) = 0;
 
     /**
+     * Equivalent to ComputePath, but using the JPS pathfinder.
+     */
+    virtual void ComputePathJPS(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, Path& ret) = 0;
+
+    /**
      * Asynchronous version of ComputePath.
      * The result will be sent as CMessagePathResult to 'notify'.
      * 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.
@@ -121 +108 @@
      * 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, Path& ret) = 0;
 
     /**
      * Asynchronous version of ComputeShortPath (using ControlGroupObstructionFilter).
@@ -129 +116 @@
      * 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;
+    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;
 
     /**
-     * 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 CFixedVector2D GetNearestPointOnGoal(CFixedVector2D pos, const PathGoal& goal) = 0;
 
     /**
      * Check whether the given movement line is valid and doesn't hit any obstructions
@@ -178 +159 @@
      */
     virtual void ProcessSameTurnMoves() = 0;
 
+    /**
+     * Returns some stats about the last ComputePath.
+     */
+    virtual void GetDebugData(u32& steps, double& time, Grid<u8>& grid) = 0;
+
+    /**
+     * Returns some stats about the last ComputePathJPS.
+     */
+    virtual void GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid) = 0;
+
     DECLARE_INTERFACE_TYPE(Pathfinder)
 };
 

source/simulation2/components/ICmpObstructionManager.h

@@ -48 +48 @@
  * 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
 {
 public:
     /**
+     * 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).
+     */
+    static const int NAVCELLS_PER_TILE = 4;
+
+    /**
+     * Size of a navcell in metres ( = TERRAIN_TILE_SIZE / NAVCELLS_PER_TILE)
+     */
+    static const fixed NAVCELL_SIZE;
+
+    /**
      * External identifiers for shapes.
      * (This is a struct rather than a raw u32 for type-safety.)
      */
@@ -205 +222 @@
         std::vector<entity_id_t>* out) = 0;
 
     /**
-     * Bit-flags for Rasterise.
+     * Bit-flags for Rasterize.
      */
     enum TileObstruction
     {
@@ -215 +232 @@
     };
 
     /**
-     * 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<u8>& grid) = 0;
+    virtual void Rasterize(Grid<u16>& grid, entity_pos_t expand, ICmpObstructionManager::flags_t requireMask, u16 setMask) = 0;
 
     /**
+     * Returns whether obstructions have changed such that Rasterize will
+     * return different data, since the last call to NeedUpdate with the same
+     * @p dirtyID handle.
+     * This should be first called with @p dirtyID pointing to 0; the function
+     * will return true and update the pointed-at value to indicate the current
+     * state. Pass the same pointer to subsequent calls, and the function either
+     * will return false (if nothing relevant has changed), or will update the
+     * value and return true (in which case you should call Rasterize again).
+     */
+    virtual bool NeedUpdate(size_t* dirtyID) = 0;
+
+    /**
      * Standard representation for all types of shapes, for use with geometry processing code.
      */
     struct ObstructionSquare
@@ -248 +274 @@
     virtual void GetObstructionsInRange(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, std::vector<ObstructionSquare>& 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.
+     * (TODO: This currently ignores the clearance/radius values of units,
+     * so it just uses the unexpanded obstruction shapes, which is not ideal.)
      */
-    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<entity_id_t>& out) = 0;
 
     /**
      * Get the obstruction square representing the given shape.
@@ -344 +370 @@
     {
         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;
     }
 };

source/simulation2/components/ICmpObstructionManager.cpp

@@ -21 +21 @@
 
 #include "simulation2/system/InterfaceScripted.h"
 
+#include "graphics/Terrain.h"
+
+const fixed ICmpObstructionManager::NAVCELL_SIZE = fixed::FromInt(TERRAIN_TILE_SIZE) / NAVCELLS_PER_TILE;
+
 BEGIN_INTERFACE_WRAPPER(ObstructionManager)
 DEFINE_INTERFACE_METHOD_1("SetPassabilityCircular", void, ICmpObstructionManager, SetPassabilityCircular, bool)
 DEFINE_INTERFACE_METHOD_1("SetDebugOverlay", void, ICmpObstructionManager, SetDebugOverlay, bool)

source/simulation2/components/CCmpUnitMotion.cpp

@@ -38 +38 @@
 #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
@@ -98 +102 @@
 static const CColor OVERLAY_COLOUR_LONG_PATH(1, 1, 1, 1);
 static const CColor OVERLAY_COLOUR_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 = ICmpObstructionManager::NAVCELL_SIZE; // for extending the goal outwards/inwards a little bit
 
 class CCmpUnitMotion : public ICmpUnitMotion
 {
@@ -123 +127 @@
     fixed m_WalkSpeed; // in metres per second
     fixed m_RunSpeed;
     ICmpPathfinder::pass_class_t m_PassClass;
-    ICmpPathfinder::cost_class_t m_CostClass;
 
     // Dynamic state:
 
@@ -236 +239 @@
     ICmpPathfinder::Path m_LongPath;
     ICmpPathfinder::Path m_ShortPath;
 
-    ICmpPathfinder::Goal m_FinalGoal;
+    PathGoal m_FinalGoal;
 
     static std::string GetSchema()
     {
@@ -245 +248 @@
             "<a:example>"
                 "<WalkSpeed>7.0</WalkSpeed>"
                 "<PassabilityClass>default</PassabilityClass>"
-                "<CostClass>infantry</CostClass>"
             "</a:example>"
             "<element name='FormationController'>"
                 "<data type='boolean'/>"
@@ -266 +268 @@
             "</optional>"
             "<element name='PassabilityClass' a:help='Identifies the terrain passability class (values are defined in special/pathfinder.xml)'>"
                 "<text/>"
-            "</element>"
-            "<element name='CostClass' a:help='Identifies the movement speed/cost class (values are defined in special/pathfinder.xml)'>"
-                "<text/>"
             "</element>";
     }
 
@@ -298 +297 @@
         if (cmpPathfinder)
         {
             m_PassClass = cmpPathfinder->GetPassabilityClass(paramNode.GetChild("PassabilityClass").ToUTF8());
-            m_CostClass = cmpPathfinder->GetCostClass(paramNode.GetChild("CostClass").ToUTF8());
         }
 
         CmpPtr<ICmpObstruction> cmpObstruction(GetSimContext(), GetEntityId());
@@ -312 +310 @@
 
         m_TargetEntity = INVALID_ENTITY;
 
-        m_FinalGoal.type = ICmpPathfinder::Goal::POINT;
+        m_FinalGoal.type = PathGoal::POINT;
 
         m_DebugOverlayEnabled = false;
     }
@@ -568 +566 @@
      * Might go in a straight line immediately, or might start an asynchronous
      * path request.
      */
-    void BeginPathing(CFixedVector2D from, const ICmpPathfinder::Goal& goal);
+    void BeginPathing(CFixedVector2D from, const PathGoal& goal);
 
     /**
      * Start an asynchronous long path query.
      */
-    void RequestLongPath(CFixedVector2D from, const ICmpPathfinder::Goal& goal);
+    void RequestLongPath(CFixedVector2D from, const PathGoal& goal);
 
     /**
      * Start an asynchronous short path query.
      */
-    void RequestShortPath(CFixedVector2D from, const ICmpPathfinder::Goal& goal, bool avoidMovingUnits);
+    void RequestShortPath(CFixedVector2D from, const PathGoal& goal, bool avoidMovingUnits);
 
     /**
      * Select a next long waypoint, given the current unit position.
@@ -824 +822 @@
         // 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);
 
@@ -1025 +1024 @@
         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
@@ -1147 +1146 @@
 
 
 
-void CCmpUnitMotion::BeginPathing(CFixedVector2D from, const ICmpPathfinder::Goal& goal)
+void CCmpUnitMotion::BeginPathing(CFixedVector2D from, const PathGoal& goal)
 {
     // Cancel any pending path requests
     m_ExpectedPathTicket = 0;
@@ -1160 +1159 @@
     if (cmpObstruction)
         cmpObstruction->SetMovingFlag(true);
 
+#if DISABLE_PATHFINDER
+    {
+        CmpPtr<ICmpPathfinder> cmpPathfinder (GetSimContext(), SYSTEM_ENTITY);
+        CFixedVector2D goalPos = cmpPathfinder->GetNearestPointOnGoal(from, m_FinalGoal);
+        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_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.
@@ -1181 +1193 @@
     RequestLongPath(from, goal);
 }
 
-void CCmpUnitMotion::RequestLongPath(CFixedVector2D from, const ICmpPathfinder::Goal& goal)
+void CCmpUnitMotion::RequestLongPath(CFixedVector2D from, const PathGoal& goal)
 {
     CmpPtr<ICmpPathfinder> cmpPathfinder(GetSimContext(), SYSTEM_ENTITY);
     if (!cmpPathfinder)
         return;
 
-    cmpPathfinder->SetDebugPath(from.X, from.Y, goal, m_PassClass, m_CostClass);
+    cmpPathfinder->SetDebugPath(from.X, from.Y, goal, m_PassClass);
 
-    m_ExpectedPathTicket = cmpPathfinder->ComputePathAsync(from.X, from.Y, goal, m_PassClass, m_CostClass, GetEntityId());
+    m_ExpectedPathTicket = cmpPathfinder->ComputePathAsync(from.X, from.Y, goal, m_PassClass, GetEntityId());
 }
 
-void CCmpUnitMotion::RequestShortPath(CFixedVector2D from, const ICmpPathfinder::Goal& goal, bool avoidMovingUnits)
+void CCmpUnitMotion::RequestShortPath(CFixedVector2D from, const PathGoal& goal, bool avoidMovingUnits)
 {
     CmpPtr<ICmpPathfinder> cmpPathfinder(GetSimContext(), SYSTEM_ENTITY);
     if (!cmpPathfinder)
@@ -1227 +1239 @@
 
     // Now we need to recompute a short path to the waypoint
 
-    ICmpPathfinder::Goal goal;
+    PathGoal goal;
     if (m_LongPath.m_Waypoints.empty())
     {
         // This was the last waypoint - head for the exact goal
@@ -1236 +1248 @@
     else
     {
         // Head for somewhere near the waypoint (but allow some leeway in case it's obstructed)
-        goal.type = ICmpPathfinder::Goal::CIRCLE;
+        goal.type = PathGoal::CIRCLE;
         goal.hw = SHORT_PATH_GOAL_RADIUS;
         goal.x = targetX;
         goal.z = targetZ;
@@ -1262 +1274 @@
 
     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<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
-        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::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 outwards 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
         {
@@ -1315 +1316 @@
             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;
@@ -1349 +1341 @@
     bool hasObstruction = false;
     CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
     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)
     {
@@ -1422 +1414 @@
     if (cmpObstruction)
         hasObstruction = cmpObstruction->GetObstructionSquare(obstruction);
 
+    if (!hasObstruction)
+    {
+        // The target didn't have an obstruction or obstruction shape, so treat it as a point instead
+
+        CmpPtr<ICmpPosition> cmpTargetPosition(GetSimContext(), target);
+        if (!cmpTargetPosition || !cmpTargetPosition->IsInWorld())
+            return false;
+
+        CFixedVector2D targetPos = cmpTargetPosition->GetPosition2D();
+
+        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.
@@ -1447 +1452 @@
      * (Those units should set minRange to 0 so they'll never be considered *too* close.)
      */
 
-    if (hasObstruction)
+    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)
     {
-        CFixedVector2D halfSize(obstruction.hw, obstruction.hh);
-        ICmpPathfinder::Goal goal;
-        goal.x = obstruction.x;
-        goal.z = obstruction.z;
+        // Too close to the square - need to move away
 
-        entity_pos_t distance = Geometry::DistanceToSquare(pos - CFixedVector2D(obstruction.x, obstruction.z), obstruction.u, obstruction.v, halfSize);
+        // TODO: maybe we should do the ShouldTreatTargetAsCircle thing here?
 
-        if (distance < minRange)
-        {
-            // Too close to the square - need to move away
+        entity_pos_t goalDistance = minRange + g_GoalDelta;
 
-            // TODO: maybe we should do the ShouldTreatTargetAsCircle thing here?
+        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:
 
-            entity_pos_t goalDistance = minRange + g_GoalDelta;
+        // Circumscribe the square
+        entity_pos_t circleRadius = halfSize.Length();
 
-            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)
+        if (ShouldTreatTargetAsCircle(maxRange, obstruction.hw, obstruction.hh, circleRadius))
         {
-            // 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:
+            // The target is small relative to our range, so pretend it's a circle
 
-            // Circumscribe the square
-            entity_pos_t circleRadius = halfSize.Length();
+            // 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 (ShouldTreatTargetAsCircle(maxRange, obstruction.hw, obstruction.hh, circleRadius))
+            if (circleDistance < maxRange)
             {
-                // 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
-                    FaceTowardsPointFromPos(pos, goal.x, goal.z);
-                    return false;
-                }
-
-                entity_pos_t goalDistance = maxRange - g_GoalDelta;
-
-                goal.type = ICmpPathfinder::Goal::CIRCLE;
-                goal.hw = circleRadius + goalDistance;
+                // We're already in range - no need to move anywhere
+                FaceTowardsPointFromPos(pos, goal.x, goal.z);
+                return false;
             }
-            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)
+            entity_pos_t goalDistance = maxRange - 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;
-            }
+            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
 
-        m_State = STATE_INDIVIDUAL_PATH;
-        m_TargetEntity = target;
-        m_TargetOffset = CFixedVector2D();
-        m_TargetMinRange = minRange;
-        m_TargetMaxRange = maxRange;
-        m_FinalGoal = goal;
+            entity_pos_t goalDistance = (maxRange - g_GoalDelta)*2 / 3; // multiply by slightly less than 1/sqrt(2)
 
-        BeginPathing(pos, goal);
-
-        return true;
+            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
-    {
-        // The target didn't have an obstruction or obstruction shape, so treat it as a point instead
 
-        CmpPtr<ICmpPosition> cmpTargetPosition(GetSimContext(), target);
-        if (!cmpTargetPosition || !cmpTargetPosition->IsInWorld())
-            return false;
+    m_State = STATE_INDIVIDUAL_PATH;
+    m_TargetEntity = target;
+    m_TargetOffset = CFixedVector2D();
+    m_TargetMinRange = minRange;
+    m_TargetMaxRange = maxRange;
+    m_FinalGoal = goal;
 
-        CFixedVector2D targetPos = cmpTargetPosition->GetPosition2D();
+    BeginPathing(pos, goal);
 
-        return MoveToPointRange(targetPos.X, targetPos.Y, minRange, maxRange);
-    }
+    return true;
 }
 
 bool CCmpUnitMotion::IsInTargetRange(entity_id_t target, entity_pos_t minRange, entity_pos_t maxRange)
@@ -1620 +1610 @@
 
     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;
 

source/simulation2/components/CCmpTerritoryManager.cpp

@@ -48 +48 @@
 
 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
@@ -269 +268 @@
 
 REGISTER_COMPONENT_TYPE(TerritoryManager)
 
+/**
+ * Compute the tile indexes on the grid nearest to a given point
+ */
+static void NearestTerritoryTile(entity_pos_t x, entity_pos_t z, int& i, int& j, int w, int h)
+{
+    i = clamp((x / (ICmpObstructionManager::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE)).ToInt_RoundToNegInfinity(), 0, w-1);
+    j = clamp((z / (ICmpObstructionManager::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE)).ToInt_RoundToNegInfinity(), 0, h-1);
+}
+
+/**
+ * Returns the position of the center of the given tile
+ */
+static void TerritoryTileCenter(int i, int j, entity_pos_t& x, entity_pos_t& z)
+{
+    x = entity_pos_t::FromInt(i*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE / 2);
+    z = entity_pos_t::FromInt(j*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE * ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE / 2);
+}
+
 /*
 We compute the territory influence of an entity with a kind of best-first search,
 storing an 'open' list of tiles that have not yet been processed,
@@ -279 +296 @@
 
 typedef PriorityQueueHeap<std::pair<u16, u16>, u32, std::greater<u32> > OpenQueue;
 
-static void ProcessNeighbour(u32 falloff, u16 i, u16 j, u32 pg, bool diagonal,
+static void ProcessNeighbour(u32 falloff, int i, int j, u32 pg, bool diagonal,
         Grid<u32>& grid, OpenQueue& queue, const Grid<u8>& costGrid)
 {
     u32 dg = falloff * costGrid.get(i, j);
@@ -294 +311 @@
     u32 g = pg - dg; // cost to this tile = cost to predecessor - falloff from predecessor
 
     grid.set(i, j, g);
-    OpenQueue::Item tile = { std::make_pair(i, j), g };
+    OpenQueue::Item tile = { std::make_pair((u16)i, (u16)j), g };
     queue.push(tile);
 }
 
 static void FloodFill(Grid<u32>& grid, Grid<u8>& costGrid, OpenQueue& openTiles, u32 falloff)
 {
-    u16 tilesW = grid.m_W;
-    u16 tilesH = grid.m_H;
+    int tilesW = grid.m_W;
+    int tilesH = grid.m_H;
 
     while (!openTiles.empty())
     {
         OpenQueue::Item tile = openTiles.pop();
 
         // Process neighbours (if they're not off the edge of the map)
-        u16 x = tile.id.first;
-        u16 z = tile.id.second;
+        int x = tile.id.first;
+        int z = tile.id.second;
         if (x > 0)
-            ProcessNeighbour(falloff, (u16)(x-1), z, tile.rank, false, grid, openTiles, costGrid);
+            ProcessNeighbour(falloff, x-1, z, tile.rank, false, grid, openTiles, costGrid);
         if (x < tilesW-1)
-            ProcessNeighbour(falloff, (u16)(x+1), z, tile.rank, false, grid, openTiles, costGrid);
+            ProcessNeighbour(falloff, x+1, z, tile.rank, false, grid, openTiles, costGrid);
         if (z > 0)
-            ProcessNeighbour(falloff, x, (u16)(z-1), tile.rank, false, grid, openTiles, costGrid);
+            ProcessNeighbour(falloff, x, z-1, tile.rank, false, grid, openTiles, costGrid);
         if (z < tilesH-1)
-            ProcessNeighbour(falloff, x, (u16)(z+1), tile.rank, false, grid, openTiles, costGrid);
+            ProcessNeighbour(falloff, x, z+1, tile.rank, false, grid, openTiles, costGrid);
         if (x > 0 && z > 0)
-            ProcessNeighbour(falloff, (u16)(x-1), (u16)(z-1), tile.rank, true, grid, openTiles, costGrid);
+            ProcessNeighbour(falloff, x-1, z-1, tile.rank, true, grid, openTiles, costGrid);
         if (x > 0 && z < tilesH-1)
-            ProcessNeighbour(falloff, (u16)(x-1), (u16)(z+1), tile.rank, true, grid, openTiles, costGrid);
+            ProcessNeighbour(falloff, x-1, z+1, tile.rank, true, grid, openTiles, costGrid);
         if (x < tilesW-1 && z > 0)
-            ProcessNeighbour(falloff, (u16)(x+1), (u16)(z-1), tile.rank, true, grid, openTiles, costGrid);
+            ProcessNeighbour(falloff, x+1, z-1, tile.rank, true, grid, openTiles, costGrid);
         if (x < tilesW-1 && z < tilesH-1)
-            ProcessNeighbour(falloff, (u16)(x+1), (u16)(z+1), tile.rank, true, grid, openTiles, costGrid);
+            ProcessNeighbour(falloff, x+1, z+1, tile.rank, true, grid, openTiles, costGrid);
     }
 }
 
@@ -343 +360 @@
     if (!cmpTerrain->IsLoaded())
         return;
 
-    u16 tilesW = cmpTerrain->GetTilesPerSide();
-    u16 tilesH = cmpTerrain->GetTilesPerSide();
+    CmpPtr<ICmpPathfinder> cmpPathfinder(GetSimContext(), SYSTEM_ENTITY);
+    ICmpPathfinder::pass_class_t passClassDefault = cmpPathfinder->GetPassabilityClass("territory");
+    ICmpPathfinder::pass_class_t passClassUnrestricted = cmpPathfinder->GetPassabilityClass("unrestricted");
 
+    const Grid<u16>& passGrid = cmpPathfinder->GetPassabilityGrid();
+
+    // Downsample the passability data to count the number of impassable
+    // navcells per territory tile
+    // (TODO: do we really want to average the passability per territory tile,
+    // instead of doing min/max/etc?)
+
+    int tilesW = passGrid.m_W / NAVCELLS_PER_TERRITORY_TILE;
+    int tilesH = passGrid.m_H / NAVCELLS_PER_TERRITORY_TILE;
+
     m_Territories = new Grid<u8>(tilesW, tilesH);
 
-    // Compute terrain-passability-dependent costs per tile
-    Grid<u8> influenceGrid(tilesW, tilesH);
+    // Downsample passability grid horizontally first
+    Grid<u16> tempPassGrid(passGrid.m_W / NAVCELLS_PER_TERRITORY_TILE, passGrid.m_H);
 
-    CmpPtr<ICmpPathfinder> cmpPathfinder(GetSimContext(), SYSTEM_ENTITY);
-    ICmpPathfinder::pass_class_t passClassDefault = cmpPathfinder->GetPassabilityClass("default");
-    ICmpPathfinder::pass_class_t passClassUnrestricted = cmpPathfinder->GetPassabilityClass("unrestricted");
+    cassert(NAVCELLS_PER_TERRITORY_TILE < 16); // else we might overflow the counters
 
-    const Grid<u16>& passGrid = cmpPathfinder->GetPassabilityGrid();
-    for (u16 j = 0; j < tilesH; ++j)
+    for (int j = 0; j < tempPassGrid.m_H; ++j)
     {
-        for (u16 i = 0; i < tilesW; ++i)
+        for (int i = 0; i < tempPassGrid.m_W; ++i)
         {
-            u16 g = passGrid.get(i, j);
+            u32 count = 0;
+            for (u16 di = 0; di < NAVCELLS_PER_TERRITORY_TILE; ++di)
+            {
+                u16 g = passGrid.get(i*NAVCELLS_PER_TERRITORY_TILE + di, j);
+                if (g & passClassUnrestricted)
+                    count += 65536; // off the world; force maximum cost
+                else if (g & passClassDefault)
+                    count += 1;
+            }
+            tempPassGrid.set(i, j, std::min((u32)65535, count)); // clamp to avoid overflow
+        }
+    }
+
+    // Compute terrain-passability-dependent costs per tile
+
+    Grid<u8> influenceGrid(tilesW, tilesH);
+    for (int j = 0; j < tilesH; ++j)
+    {
+        for (int i = 0; i < tilesW; ++i)
+        {
+            // Downsample vertically
+            u32 count = 0;
+            for (int dj = 0; dj < NAVCELLS_PER_TERRITORY_TILE; ++dj)
+                count += tempPassGrid.get(i, j*NAVCELLS_PER_TERRITORY_TILE + dj);
+
             u8 cost;
-            if (g & passClassUnrestricted)
-                cost = 255; // off the world; use maximum cost
-            else if (g & passClassDefault)
-                cost = m_ImpassableCost;
+            if (count >= 65535)
+            {
+                cost = 255; // at least partially off the world; use maximum cost
+            }
             else
-                cost = 1;
+            {
+                // Compute average cost of the cells within this tile
+                u32 totalCells = NAVCELLS_PER_TERRITORY_TILE*NAVCELLS_PER_TERRITORY_TILE;
+                cost = (m_ImpassableCost*count + 1*(totalCells-count)) / totalCells;
+            }
             influenceGrid.set(i, j, cost);
         }
     }
@@ -430 +483 @@
 
             CmpPtr<ICmpPosition> cmpPosition(GetSimContext(), *eit);
             CFixedVector2D pos = cmpPosition->GetPosition2D();
-            u16 i = (u16)clamp((pos.X / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, tilesW-1);
-            u16 j = (u16)clamp((pos.Y / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, tilesH-1);
+            int i, j;
+            NearestTerritoryTile(pos.X, pos.Y, i, j, tilesW, tilesH);
 
             CmpPtr<ICmpTerritoryInfluence> cmpTerritoryInfluence(GetSimContext(), *eit);
             u32 weight = cmpTerritoryInfluence->GetWeight();
-            u32 radius = cmpTerritoryInfluence->GetRadius() / TERRAIN_TILE_SIZE;
+            u32 radius = (fixed::FromInt(cmpTerritoryInfluence->GetRadius()) / (ICmpObstructionManager::NAVCELL_SIZE * NAVCELLS_PER_TERRITORY_TILE)).ToInt_RoundToNegInfinity();
             u32 falloff = weight / radius; // earlier check for GetRadius() == 0 prevents divide-by-zero
 
             // TODO: we should have some maximum value on weight, to avoid overflow
@@ -451 +504 @@
             FloodFill(entityGrid, influenceGrid, openTiles, falloff);
 
             // TODO: we should do a sparse grid and only add the non-zero regions, for performance
-            for (u16 j = 0; j < entityGrid.m_H; ++j)
-                for (u16 i = 0; i < entityGrid.m_W; ++i)
+            for (int j = 0; j < entityGrid.m_H; ++j)
+                for (int i = 0; i < entityGrid.m_W; ++i)
                     playerGrid.set(i, j, playerGrid.get(i, j) + entityGrid.get(i, j));
         }
 
@@ -460 +513 @@
     }
 
     // Set m_Territories to the player ID with the highest influence for each tile
-    for (u16 j = 0; j < tilesH; ++j)
+    for (int j = 0; j < tilesH; ++j)
     {
-        for (u16 i = 0; i < tilesW; ++i)
+        for (int i = 0; i < tilesW; ++i)
         {
             u32 bestWeight = 0;
             for (size_t k = 0; k < playerGrids.size(); ++k)
@@ -487 +540 @@
         CmpPtr<ICmpPosition> cmpPosition(GetSimContext(), *it);
 
         CFixedVector2D pos = cmpPosition->GetPosition2D();
-        u16 i = (u16)clamp((pos.X / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, tilesW-1);
-        u16 j = (u16)clamp((pos.Y / (int)TERRAIN_TILE_SIZE).ToInt_RoundToNegInfinity(), 0, tilesH-1);
+        int i, j;
+        NearestTerritoryTile(pos.X, pos.Y, i, j, tilesW, tilesH);
 
         u8 owner = (u8)cmpOwnership->GetOwner();
 
@@ -500 +553 @@
 
         Grid<u8>& grid = *m_Territories;
 
-        u16 maxi = (u16)(grid.m_W-1);
-        u16 maxj = (u16)(grid.m_H-1);
+        int maxi = grid.m_W-1;
+        int maxj = grid.m_H-1;
 
         std::vector<std::pair<u16, u16> > tileStack;
 
-#define MARK_AND_PUSH(i, j) STMT(grid.set(i, j, owner | TERRITORY_CONNECTED_MASK); tileStack.push_back(std::make_pair(i, j)); )
+#define MARK_AND_PUSH(i, j) STMT(grid.set(i, j, owner | TERRITORY_CONNECTED_MASK); tileStack.push_back(std::make_pair((u16)(i), (u16)(j))); )
 
         MARK_AND_PUSH(i, j);
         while (!tileStack.empty())
@@ -537 +590 @@
     }
 }
 
-/**
- * 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)
-{
-    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);
-}
-
-/**
- * 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);
-}
-
-// TODO: would be nice not to duplicate those two functions from CCmpObstructionManager.cpp
-
-
 void CCmpTerritoryManager::RasteriseInfluences(CComponentManager::InterfaceList& infls, Grid<u8>& grid)
 {
     for (CComponentManager::InterfaceList::iterator it = infls.begin(); it != infls.end(); ++it)
@@ -579 +611 @@
         CFixedVector2D halfSize(square.hw, square.hh);
         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, grid.m_W, grid.m_H);
-        NearestTile(square.x + halfBound.X, square.z + halfBound.Y, i1, j1, grid.m_W, grid.m_H);
-        for (u16 j = j0; j <= j1; ++j)
+        int i0, j0, i1, j1;
+        NearestTerritoryTile(square.x - halfBound.X, square.z - halfBound.Y, i0, j0, grid.m_W, grid.m_H);
+        NearestTerritoryTile(square.x + halfBound.X, square.z + halfBound.Y, i1, j1, grid.m_W, grid.m_H);
+        for (int j = j0; j <= j1; ++j)
         {
-            for (u16 i = i0; i <= i1; ++i)
+            for (int i = i0; i <= i1; ++i)
             {
                 entity_pos_t x, z;
-                TileCenter(i, j, x, z);
+                TerritoryTileCenter(i, j, x, z);
                 if (Geometry::PointIsInSquare(CFixedVector2D(x - square.x, z - square.z), square.u, square.v, halfSize))
                     grid.set(i, j, (u8)cost);
             }
@@ -714 +746 @@
 
 player_id_t CCmpTerritoryManager::GetOwner(entity_pos_t x, entity_pos_t z)
 {
-    u16 i, j;
     CalculateTerritories();
     if (!m_Territories)
         return 0;
 
-    NearestTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H);
+    int i, j;
+    NearestTerritoryTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H);
     return m_Territories->get(i, j) & TERRITORY_PLAYER_MASK;
 }
 
 bool CCmpTerritoryManager::IsConnected(entity_pos_t x, entity_pos_t z)
 {
-    u16 i, j;
     CalculateTerritories();
     if (!m_Territories)
         return false;
 
-    NearestTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H);
+    int i, j;
+    NearestTerritoryTile(x, z, i, j, m_Territories->m_W, m_Territories->m_H);
     return (m_Territories->get(i, j) & TERRITORY_CONNECTED_MASK) != 0;
 }
 
-TerritoryOverlay::TerritoryOverlay(CCmpTerritoryManager& manager)
-    : TerrainOverlay(manager.GetSimContext()), m_TerritoryManager(manager)
-{ }
-
-void TerritoryOverlay::StartRender()
+TerritoryOverlay::TerritoryOverlay(CCmpTerritoryManager& manager) :
+    TerrainTextureOverlay((float)ICmpObstructionManager::NAVCELLS_PER_TILE / ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE),
+    m_TerritoryManager(manager)
 {
-    m_TerritoryManager.CalculateTerritories();
 }
 
-void TerritoryOverlay::ProcessTile(ssize_t i, ssize_t j)
+void TerritoryOverlay::BuildTextureRGBA(u8* data, size_t w, size_t h)
 {
-    if (!m_TerritoryManager.m_Territories)
-        return;
+    m_TerritoryManager.CalculateTerritories();
 
-    u8 id = (m_TerritoryManager.m_Territories->get((int) i, (int) j) & ICmpTerritoryManager::TERRITORY_PLAYER_MASK);
+    for (size_t j = 0; j < h; ++j)
+    {
+        for (size_t i = 0; i < w; ++i)
+        {
+            SColor4ub color;
 
-    float a = 0.2f;
-    switch (id)
-    {
-    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;
+            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;
+        }
     }
 }

source/simulation2/components/CCmpRangeManager.cpp

@@ -22 +22 @@
 
 #include "simulation2/MessageTypes.h"
 #include "simulation2/components/ICmpPosition.h"
+#include "simulation2/components/ICmpObstructionManager.h"
 #include "simulation2/components/ICmpTerritoryManager.h"
 #include "simulation2/components/ICmpVision.h"
 #include "simulation2/helpers/Render.h"
@@ -1072 +1073 @@
         if (!cmpTerritoryManager || !cmpTerritoryManager->NeedUpdate(&m_TerritoriesDirtyID))
             return;
 
+        PROFILE3("UpdateTerritoriesLos");
+
         const Grid<u8>& 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 >= ICmpObstructionManager::NAVCELLS_PER_TILE);
+        cassert(ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE % ICmpObstructionManager::NAVCELLS_PER_TILE == 0);
+
+        int scale = ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE / ICmpObstructionManager::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)
         {
             for (u16 i = 0; i < grid.m_W; ++i)
@@ -1085 +1099 @@
                 u8 p = grid.get(i, j) & ICmpTerritoryManager::TERRITORY_PLAYER_MASK;
                 if (p > 0 && p <= MAX_LOS_PLAYER_ID)
                 {
-                    m_LosState[i + j*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
-                    m_LosState[i+1 + j*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
-                    m_LosState[i + (j+1)*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
-                    m_LosState[i+1 + (j+1)*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
+                    for (int dj = 0; dj <= scale; ++dj)
+                        for (int di = 0; di <= scale; ++di)
+                            m_LosState[(i*scale+di) + (j*scale+dj)*m_TerrainVerticesPerSide] |= (LOS_EXPLORED << (2*(p-1)));
                 }
             }
         }

source/simulation2/components/CCmpRallyPointRenderer.cpp

@@ -69 +69 @@
 {
     // import some types for less verbosity
     typedef ICmpPathfinder::Path Path;
-    typedef ICmpPathfinder::Goal Goal;
+    typedef PathGoal Goal;
     typedef ICmpPathfinder::Waypoint Waypoint;
     typedef ICmpRangeManager::CLosQuerier CLosQuerier;
     typedef SOverlayTexturedLine::LineCapType LineCapType;
@@ -115 +115 @@
     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;
@@ -141 +140 @@
                 "<LineEndCap>square</LineEndCap>"
                 "<LineColour r='20' g='128' b='240'></LineColour>"
                 "<LineDashColour r='158' g='11' b='15'></LineDashColour>"
-                "<LineCostClass>default</LineCostClass>"
                 "<LinePassabilityClass>default</LinePassabilityClass>"
             "</a:example>"
             "<element name='MarkerTemplate' a:help='Template name for the rally point marker entity (typically a waypoint flag actor)'>"
@@ -196 +194 @@
             "</element>"
             "<element name='LinePassabilityClass' a:help='The pathfinder passability class to use for computing the rally point marker line path'>"
                 "<text />"
-            "</element>"
-            "<element name='LineCostClass' a:help='The pathfinder cost class to use for computing the rally point marker line path'>"
-                "<text />"
             "</element>";
     }
 
@@ -440 +435 @@
     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();
 
     // ---------------------------------------------------------------------------------------------
@@ -612 +606 @@
         pathStartY,
         goal,
         cmpPathfinder->GetPassabilityClass(m_LinePassabilityClass),
-        cmpPathfinder->GetCostClass(m_LineCostClass),
         path
     );
 

source/simulation2/components/CCmpPathfinder_Vertex.cpp

@@ -101 +101 @@
 };
 
 // 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.
@@ -284 +290 @@
 }
 
 
-static CFixedVector2D NearestPointOnGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal)
+static CFixedVector2D NearestPointOnGoal(CFixedVector2D pos, const PathGoal& goal)
 {
     CFixedVector2D g(goal.x, goal.z);
 
     switch (goal.type)
     {
-    case CCmpPathfinder::Goal::POINT:
+    case PathGoal::POINT:
     {
         return g;
     }
 
-    case CCmpPathfinder::Goal::CIRCLE:
+    case PathGoal::CIRCLE:
+    case PathGoal::INVERTED_CIRCLE:
     {
         CFixedVector2D d = pos - g;
         if (d.IsZero())
@@ -304 +311 @@
         return g + d;
     }
 
-    case CCmpPathfinder::Goal::SQUARE:
+    case PathGoal::SQUARE:
+    case PathGoal::INVERTED_SQUARE:
     {
         CFixedVector2D halfSize(goal.hw, goal.hh);
         CFixedVector2D d = pos - g;
@@ -317 +325 @@
     }
 }
 
-CFixedVector2D CCmpPathfinder::GetNearestPointOnGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal)
+CFixedVector2D CCmpPathfinder::GetNearestPointOnGoal(CFixedVector2D pos, const PathGoal& goal)
 {
     return NearestPointOnGoal(pos, goal);
     // (It's intentional that we don't put the implementation inside this
@@ -327 +335 @@
 
 typedef PriorityQueueHeap<u16, fixed> PriorityQueue;
 
-struct TileEdge
+/**
+ * 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<Edge>& edges, std::vector<Vertex>& vertexes,
+    int i0, int j0, int i1, int j1,
+    ICmpPathfinder::pass_class_t passClass, const Grid<NavcellData>& grid)
 {
-    u16 i, j;
-    enum { TOP, BOTTOM, LEFT, RIGHT } dir;
-};
-
-static void AddTerrainEdges(std::vector<Edge>& edgesAA, std::vector<Vertex>& vertexes,
-    u16 i0, u16 j0, u16 i1, u16 j1, fixed r,
-    ICmpPathfinder::pass_class_t passClass, const Grid<TerrainTile>& terrain)
-{
     PROFILE("AddTerrainEdges");
 
-    std::vector<TileEdge> 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(ICmpObstructionManager::NAVCELL_SIZE);
+                vertexes.push_back(vert);
+            }
 
-                if (j > 0 && IS_TERRAIN_PASSABLE(terrain.get(i, j-1), passClass))
-                {
-                    TileEdge e = { i, j, TileEdge::BOTTOM };
-                    tileEdges.push_back(e);
-                    any = true;
-                }
+            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(ICmpObstructionManager::NAVCELL_SIZE);
+                vertexes.push_back(vert);
+            }
 
-                if (j < terrain.m_H-1 && IS_TERRAIN_PASSABLE(terrain.get(i, j+1), passClass))
-                {
-                    TileEdge e = { i, j, TileEdge::TOP };
-                    tileEdges.push_back(e);
-                    any = true;
-                }
+            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(ICmpObstructionManager::NAVCELL_SIZE);
+                vertexes.push_back(vert);
+            }
 
-                if (i > 0 && IS_TERRAIN_PASSABLE(terrain.get(i-1, j), 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_TR;
+                vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+                vertexes.push_back(vert);
+            }
+        }
+    }
+
+    // XXX rewrite this stuff
+
+    for (int j = j0; j < j1; ++j)
+    {
+        std::vector<u16> segmentsR;
+        std::vector<u16> 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::LEFT };
-                    tileEdges.push_back(e);
-                    any = true;
-                }
+                    CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+                    CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+                    Edge e = { v0, v1 };
+                    edges.push_back(e);
 
-                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;
+                    ia = segmentsR[n];
+                    ib = ia + 1;
                 }
+            }
+        }
 
-                // 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)
+        if (!segmentsL.empty())
+        {
+            segmentsL.push_back(0); // sentinel value to simplify the loop
+            u16 ia = segmentsL[0];
+            u16 ib = ia + 1;
+            for (size_t n = 1; n < segmentsL.size(); ++n)
+            {
+                if (segmentsL[n] == ib)
+                    ++ib;
+                else
                 {
-                    CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(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);
+                    CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+                    CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
                     Edge e = { v0, v1 };
-                    edgesAA.push_back(e);
+                    edges.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<u16> segmentsU;
+        std::vector<u16> 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(ICmpObstructionManager::NAVCELL_SIZE);
+                    CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+                    Edge e = { v0, v1 };
+                    edges.push_back(e);
+
+                    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;
+            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(ICmpObstructionManager::NAVCELL_SIZE);
+                    CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+                    Edge e = { v0, v1 };
+                    edges.push_back(e);
+
+                    ja = segmentsD[n];
+                    jb = ja + 1;
+                }
+            }
         }
-        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;
-        }
-        }
     }
 }
 
 static void SplitAAEdges(CFixedVector2D a,
-        const std::vector<Edge>& edgesAA,
+        const std::vector<Edge>& edges,
+        const std::vector<Square>& squares,
+        std::vector<Edge>& edgesUnaligned,
         std::vector<EdgeAA>& edgesLeft, std::vector<EdgeAA>& edgesRight,
         std::vector<EdgeAA>& edgesBottom, std::vector<EdgeAA>& 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 (size_t i = 0; i < squares.size(); ++i)
     {
-        if (a.X <= edgesAA[i].p0.X)
+        if (a.X <= squares[i].p0.X)
         {
-            EdgeAA e = { edgesAA[i].p0, edgesAA[i].p1.Y };
+            EdgeAA e = { squares[i].p0, squares[i].p1.Y };
             edgesLeft.push_back(e);
         }
-        if (a.X >= edgesAA[i].p1.X)
+        if (a.X >= squares[i].p1.X)
         {
-            EdgeAA e = { edgesAA[i].p1, edgesAA[i].p0.Y };
+            EdgeAA e = { squares[i].p1, squares[i].p0.Y };
             edgesRight.push_back(e);
         }
-        if (a.Y <= edgesAA[i].p0.Y)
+        if (a.Y <= squares[i].p0.Y)
         {
-            EdgeAA e = { edgesAA[i].p0, edgesAA[i].p1.X };
+            EdgeAA e = { squares[i].p0, squares[i].p1.X };
             edgesBottom.push_back(e);
         }
-        if (a.Y >= edgesAA[i].p1.Y)
+        if (a.Y >= squares[i].p1.Y)
         {
-            EdgeAA e = { edgesAA[i].p1, edgesAA[i].p0.X };
+            EdgeAA e = { squares[i].p1, squares[i].p0.X };
             edgesTop.push_back(e);
         }
     }
+
+    for (size_t i = 0; i < edges.size(); ++i)
+    {
+        if (edges[i].p0.X == edges[i].p1.X)
+        {
+            if (edges[i].p1.Y < edges[i].p0.Y)
+            {
+                if (!(a.X <= edges[i].p0.X))
+                    continue;
+                EdgeAA e = { edges[i].p1, edges[i].p0.Y };
+                edgesLeft.push_back(e);
+            }
+            else
+            {
+                if (!(a.X >= edges[i].p0.X))
+                    continue;
+                EdgeAA e = { edges[i].p1, edges[i].p0.Y };
+                edgesRight.push_back(e);
+            }
+        }
+        else if (edges[i].p0.Y == edges[i].p1.Y)
+        {
+            if (edges[i].p0.X < edges[i].p1.X)
+            {
+                if (!(a.Y <= edges[i].p0.Y))
+                    continue;
+                EdgeAA e = { edges[i].p0, edges[i].p1.X };
+                edgesBottom.push_back(e);
+            }
+            else
+            {
+                if (!(a.Y >= edges[i].p0.Y))
+                    continue;
+                EdgeAA e = { edges[i].p0, edges[i].p1.X };
+                edgesTop.push_back(e);
+            }
+        }
+        else
+        {
+            edgesUnaligned.push_back(edges[i]);
+        }
+    }
 }
 
 /**
- * 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 +624 @@
 
 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, Path& path)
 {
     UpdateGrid(); // TODO: only need to bother updating if the terrain changed
 
     PROFILE3("ComputeShortPath");
-//  ScopeTimer UID__(L"ComputeShortPath");
+    TIMER(L"ComputeShortPath");
 
     m_DebugOverlayShortPathLines.clear();
 
@@ -511 +640 @@
         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 +664 @@
     // 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<Edge> edges;
-    std::vector<Edge> edgesAA; // axis-aligned squares
+    std::vector<Square> 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
@@ -574 +705 @@
     // 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);
+        NearestNavcell(rangeXMin, rangeZMin, i0, j0);
+        NearestNavcell(rangeXMax, rangeZMax, i1, j1);
+        AddTerrainEdges(edges, vertexes, i0, j0, i1, j1, passClass, *m_Grid);
     }
 
     // Find all the obstruction squares that might affect us
@@ -586 +717 @@
 
     // 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 +746 @@
 
         // 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));
@@ -624 +755 @@
         CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v));
         if (aa)
         {
-            Edge e = { ev1, ev3 };
-            edgesAA.push_back(e);
+            Square e = { ev1, ev3 };
+            edgeSquares.push_back(e);
         }
         else
         {
@@ -645 +776 @@
 
     ENSURE(vertexes.size() < 65536); // we store array indexes as u16
 
+    // Render the debug overlay
     if (m_DebugOverlay)
     {
-        // Render the obstruction edges
+#define PUSH_POINT(p) 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(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);
+        }
+
+        // Render the edges
         for (size_t i = 0; i < edges.size(); ++i)
         {
             m_DebugOverlayShortPathLines.push_back(SOverlayLine());
             m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
             std::vector<float> 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());
+            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
 
-        for (size_t i = 0; i < edgesAA.size(); ++i)
+        // 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<float> 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);
         }
     }
@@ -714 +884 @@
 
         // 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<Edge> edgesUnaligned;
         std::vector<EdgeAA> edgesLeft;
         std::vector<EdgeAA> edgesRight;
         std::vector<EdgeAA> edgesBottom;
         std::vector<EdgeAA> edgesTop;
-        SplitAAEdges(vertexes[curr.id].p, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
+        SplitAAEdges(vertexes[curr.id].p, edges, edgeSquares, edgesUnaligned, edgesLeft, edgesRight, edgesBottom, edgesTop);
+        //debug_printf(L"edges: e=%d aa=%d; u=%d l=%d r=%d b=%d t=%d\n", edges.size(), edgeSquares.size(), edgesUnaligned.size(), edgesLeft.size(), edgesRight.size(), edgesBottom.size(), edgesTop.size());
 
         // Check the lines to every other vertex
         for (size_t n = 0; n < vertexes.size(); ++n)
@@ -768 +940 @@
                 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 +964 @@
                     // 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
@@ -823 +995 @@
                         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 +1007 @@
                     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);
                 }
             }
         }
@@ -854 +1027 @@
     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<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
     if (!cmpObstructionManager)
         return false;
@@ -861 +1036 @@
     if (cmpObstructionManager->TestLine(filter, x0, z0, x1, z1, r))
         return false;
 
-    // Test against terrain:
+    // Test against the passability grid.
+    // This should ignore r, and just check that the line (x0,z0)-(x1,z1)
+    // does not intersect any impassable navcells.
+    // 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.
 
-    // (TODO: this could probably be a tiny bit faster by not reusing all the vertex computation code)
+    // 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).
+
     UpdateGrid();
 
-    std::vector<Edge> edgesAA;
-    std::vector<Vertex> 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);
+    NearestNavcell(x0, z0, i0, j0);
+    NearestNavcell(x1, z1, i1, j1);
 
-    CFixedVector2D a(x0, z0);
-    CFixedVector2D b(x1, z1);
+    // 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);
 
-    std::vector<EdgeAA> edgesLeft;
-    std::vector<EdgeAA> edgesRight;
-    std::vector<EdgeAA> edgesBottom;
-    std::vector<EdgeAA> edgesTop;
-    SplitAAEdges(a, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
+    u16 i = i0;
+    u16 j = j0;
 
-    bool visible =
-        CheckVisibilityLeft(a, b, edgesLeft) &&
-        CheckVisibilityRight(a, b, edgesRight) &&
-        CheckVisibilityBottom(a, b, edgesBottom) &&
-        CheckVisibilityTop(a, b, edgesTop);
+//  debug_printf(L"(%f,%f)..(%f,%f) [%d,%d]..[%d,%d]\n", x0.ToFloat(), z0.ToFloat(), x1.ToFloat(), z1.ToFloat(), i0, j0, i1, j1);
 
-    return visible;
+    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(ICmpObstructionManager::NAVCELL_SIZE);
+        entity_pos_t xib = entity_pos_t::FromInt(i+1).Multiply(ICmpObstructionManager::NAVCELL_SIZE);
+        entity_pos_t zj = entity_pos_t::FromInt(j + (dj+1)/2).Multiply(ICmpObstructionManager::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);
+
+//      debug_printf(L"(%f,%f)-(%f,%f) :: %f %f\n", xia.ToFloat(), zj.ToFloat(), xib.ToFloat(), zj.ToFloat(), dota.ToFloat(), dotb.ToFloat());
+
+        if ((dota < entity_pos_t::Zero() && dotb < entity_pos_t::Zero()) ||
+            (dota > entity_pos_t::Zero() && dotb > entity_pos_t::Zero()))
+        {
+            // Horizontal edge is fully on one side of the line, so the line doesn't
+            // intersect it, so we should move across the vertical edge instead
+            i += di;
+        }
+        else
+        {
+            j += dj;
+        }
+    }
 }

source/simulation2/components/CCmpPathfinder_Tile.cpp

@@ -1 @@
-/* Copyright (C) 2010 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
@@ -30 +30 @@
 #include "renderer/TerrainOverlay.h"
 #include "simulation2/helpers/PriorityQueue.h"
 
-typedef PriorityQueueHeap<std::pair<u16, u16>, u32> PriorityQueue;
+#define PATHFIND_STATS 1
 
-#define PATHFIND_STATS 0
+typedef PriorityQueueHeap<TileID, PathCost> PriorityQueue;
 
-#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)
+ * (We store an array of one of these per terrain navcell, so it ought to be optimized for size)
  */
 struct PathfindTile
 {
@@ -63 +57 @@
     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)
+    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);
+        ENSURE(pi-i == -1 || pi-i == 0 || pi-i == 1);
+        ENSURE(pj-j == -1 || pj-j == 0 || pj-j == 1);
 #endif
+        dpi = (i8)((int)pi - (int)i);
+        dpj = (i8)((int)pj - (int)j);
     }
 
+    PathCost GetCost() const { return g; }
+    void SetCost(PathCost cost) { g = cost; }
+
 private:
+    i8 dpi, dpj; // values are in {-1, 0, 1}, pointing to an adjacent tile
     u8 status; // this only needs 2 bits
-    i8 dpi, dpj; // these only really need 2 bits in total
+    PathCost g; // cost to reach this tile
+
 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; }
@@ -97 +93 @@
  * Terrain overlay for pathfinder debugging.
  * Renders a representation of the most recent pathfinding operation.
  */
-class PathfinderOverlay : public TerrainOverlay
+class PathfinderOverlay : public TerrainTextureOverlay
 {
-    NONCOPYABLE(PathfinderOverlay);
 public:
     CCmpPathfinder& m_Pathfinder;
 
-    PathfinderOverlay(CCmpPathfinder& pathfinder)
-        : TerrainOverlay(pathfinder.GetSimContext()), m_Pathfinder(pathfinder)
+    PathfinderOverlay(CCmpPathfinder& pathfinder) :
+        TerrainTextureOverlay(ICmpObstructionManager::NAVCELLS_PER_TILE), m_Pathfinder(pathfinder)
     {
     }
 
-    virtual void StartRender()
+    virtual void BuildTextureRGBA(u8* data, size_t w, size_t h)
     {
+        // Ensure m_Pathfinder.m_Grid is up-to-date
         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);
+        // Grab the debug data for the most recently generated path
+        u32 steps;
+        double time;
+        Grid<u8> debugGrid;
+        if (m_Pathfinder.m_DebugGridJPS)
+            m_Pathfinder.GetDebugDataJPS(steps, time, debugGrid);
+        else if (m_Pathfinder.m_DebugGrid)
+            m_Pathfinder.GetDebugData(steps, time, debugGrid);
 
-        if (m_Pathfinder.m_DebugGrid)
+        // Render navcell passability
+        u8* p = data;
+        for (size_t j = 0; j < h; ++j)
         {
-            PathfindTile& n = m_Pathfinder.m_DebugGrid->get((int)i, (int)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);
 
-            float c = clamp((float)n.GetStep() / (float)m_Pathfinder.m_DebugSteps, 0.f, 1.f);
+                if (debugGrid.m_W && debugGrid.m_H)
+                {
+                    u8 n = debugGrid.get((int)i, (int)j);
 
-            if (n.IsOpen())
-                RenderTile(CColor(1, 1, c, 0.6f), false);
-            else if (n.IsClosed())
-                RenderTile(CColor(0, 1, c, 0.6f), false);
+                    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;
+            }
         }
-    }
 
-    virtual void EndRender()
-    {
-        if (m_Pathfinder.m_DebugPath)
+        // Render the most recently generated path
+        if (m_Pathfinder.m_DebugPath && !m_Pathfinder.m_DebugPath->m_Waypoints.empty())
         {
-            std::vector<ICmpPathfinder::Waypoint>& wp = m_Pathfinder.m_DebugPath->m_Waypoints;
-            for (size_t n = 0; n < wp.size(); ++n)
+            std::vector<ICmpPathfinder::Waypoint>& waypoints = m_Pathfinder.m_DebugPath->m_Waypoints;
+            u16 ip = 0, jp = 0;
+            for (size_t k = 0; k < waypoints.size(); ++k)
             {
                 u16 i, j;
-                m_Pathfinder.NearestTile(wp[n].x, wp[n].z, i, j);
-                RenderTileOutline(CColor(1, 1, 1, 1), 2, false, i, j);
+                m_Pathfinder.NearestNavcell(waypoints[k].x, waypoints[k].z, i, j);
+                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);
+                }
             }
         }
     }
@@ -159 +197 @@
     }
 }
 
-void CCmpPathfinder::SetDebugPath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass)
+void CCmpPathfinder::SetDebugPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass)
 {
     if (!m_DebugOverlay)
         return;
 
-    delete m_DebugGrid;
-    m_DebugGrid = NULL;
+    SAFE_DELETE(m_DebugGrid);
     delete m_DebugPath;
     m_DebugPath = new Path();
-    ComputePath(x0, z0, goal, passClass, costClass, *m_DebugPath);
+#if PATHFIND_USE_JPS
+    ComputePathJPS(x0, z0, goal, passClass, *m_DebugPath);
+#else
+    ComputePath(x0, z0, goal, passClass, *m_DebugPath);
+#endif
     m_DebugPassClass = passClass;
 }
 
 void CCmpPathfinder::ResetDebugPath()
 {
-    delete m_DebugGrid;
-    m_DebugGrid = NULL;
-    delete m_DebugPath;
-    m_DebugPath = NULL;
+    SAFE_DELETE(m_DebugGrid);
+    SAFE_DELETE(m_DebugPath);
 }
 
 
 //////////////////////////////////////////////////////////
 
-
 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<u32> moveCosts;
 
     PriorityQueue open;
     // (there's no explicit closed list; it's encoded in PathfindTile)
 
     PathfindTileGrid* tiles;
-    Grid<TerrainTile>* terrain;
+    Grid<NavcellData>* terrain;
 
-    bool ignoreImpassable; // allows us to escape if stuck in patches of impassability
-
-    u32 hBest; // heuristic of closest discovered tile to goal
+    PathCost hBest; // heuristic of closest discovered tile to goal
     u16 iBest, jBest; // closest tile
 
 #if PATHFIND_STATS
@@ -215 +249 @@
 #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
+// Calculate heuristic cost from tile i,j to goal
 // (This ought to be an underestimate for correctness)
-static u32 CalculateHeuristic(u16 i, u16 j, u16 iGoal, u16 jGoal, u16 rGoal)
+static PathCost CalculateHeuristic(int i, int j, int iGoal, int jGoal)
 {
-#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
+    int di = abs(i - iGoal);
+    int dj = abs(j - jGoal);
+    int diag = std::min(di, dj);
+    return PathCost(di-diag + dj-diag, diag);
 }
 
-// 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)
+static void ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state)
 {
 #if PATHFIND_STATS
     state.numProcessed++;
 #endif
 
+    PathfindTile& n = state.tiles->get(i, j);
+
+    if (n.IsClosed())
+        return;
+
     // Reject impassable tiles
-    TerrainTile tileTag = state.terrain->get(i, j);
-    if (!IS_PASSABLE(tileTag, state.passClass) && !state.ignoreImpassable)
+    if (!IS_PASSABLE(state.terrain->get(i, j), state.passClass))
         return;
+    // Also, diagonal moves are only allowed if the adjacent tiles
+    // are also unobstructed
+    if (pi != i && pj != j)
+    {
+        if (!IS_PASSABLE(state.terrain->get(pi, j), state.passClass))
+            return;
+        if (!IS_PASSABLE(state.terrain->get(i, pj), state.passClass))
+            return;
+    }
 
-    u32 dg = CalculateCostDelta(pi, pj, i, j, state.tiles, state.moveCosts.at(GET_COST_CLASS(tileTag)));
+    PathCost dg;
+    if (pi == i || pj == j)
+        dg = PathCost::horizvert(1);
+    else
+        dg = PathCost::diag(1);
 
-    u32 g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor
+    PathCost g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor
 
-    PathfindTile& n = state.tiles->get(i, j);
+    PathCost h = CalculateHeuristic(i, j, state.iGoal, state.jGoal);
 
     // 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)
+        if (h < state.hBest)
         {
-            state.hBest = n.h;
+            state.hBest = h;
             state.iBest = i;
             state.jBest = j;
         }
@@ -321 +309 @@
     {
         // 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)
+        if (g >= n.GetCost())
             return;
 
         // Otherwise, we have a better path.
@@ -330 +318 @@
         if (n.IsOpen())
         {
             // This is a better path, so replace the old one with the new cost/parent
-            n.cost = g;
+            PathCost gprev = n.GetCost();
+            n.SetCost(g);
             n.SetPred(pi, pj, i, j);
             n.SetStep(state.steps);
-            state.open.promote(std::make_pair(i, j), g + n.h);
+            state.open.promote(TileID(i, j), gprev + h, g + h);
 #if PATHFIND_STATS
             state.numImproveOpen++;
 #endif
             return;
         }
 
+#if PATHFIND_STATS
         // 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
+            // by falling through
             state.numImproveClosed++;
-#endif
-            // (fall through)
         }
     }
+#endif
 
     // Add it to the open list:
     n.SetStatusOpen();
-    n.cost = g;
+    n.SetCost(g);
     n.SetPred(pi, pj, i, j);
     n.SetStep(state.steps);
-    PriorityQueue::Item t = { std::make_pair(i, j), g + n.h };
+    PriorityQueue::Item t = { TileID(i, j), g + 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)
+void CCmpPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, Path& path)
 {
     UpdateGrid();
 
     PROFILE3("ComputePath");
+    TIMER(L"ComputePath");
+    double time = timer_Time();
 
     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);
+    NearestNavcell(x0, z0, i0, j0);
 
+    // To be consistent with the JPS pathfinder (which requires passable source navcell),
+    // and to let us guarantee the goal is reachable from the source, we switch to
+    // the escape-from-impassability mode if currently on an impassable navcell
+    if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass))
+    {
+        ComputePathOffImpassable(i0, j0, passClass, path);
+        return;
+    }
+
+    // Adjust the goal so that it's reachable from the source navcell
+    PathGoal goal = origGoal;
+    PathfinderHierMakeGoalReachable(i0, j0, goal, passClass);
+
     // If we're already at the goal tile, then move directly to the exact goal coordinates
-    if (AtGoal(i0, j0, goal))
+    // XXX: this seems bogus for non-point goals, it should be the point on the current cell nearest the goal
+    if (goal.NavcellContainsGoal(i0, j0))
     {
         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;
+    // Store the navcell at the goal center, for A* heuristics
+    NearestNavcell(goal.x, goal.z, state.iGoal, state.jGoal);
 
     state.passClass = passClass;
-    state.moveCosts = m_MoveCosts.at(costClass);
 
     state.steps = 0;
 
-    state.tiles = new PathfindTileGrid(m_MapSize, m_MapSize);
+    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, state.rGoal);
+    state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal);
 
-    PriorityQueue::Item start = { std::make_pair(i0, j0), 0 };
+    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).cost = 0;
+    state.tiles->get(i0, j0).SetCost(PathCost());
 
-    // 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;
@@ -433 +422 @@
 
         // Move best tile from open to closed
         PriorityQueue::Item curr = state.open.pop();
-        u16 i = curr.id.first;
-        u16 j = curr.id.second;
+        u16 i = curr.id.i();
+        u16 j = curr.id.j();
         state.tiles->get(i, j).SetStatusClosed();
 
         // If we've reached the destination, stop
-        if (AtGoal(i, j, goal))
+        if (goal.NavcellContainsGoal(i, j))
         {
             state.iBest = i;
             state.jBest = j;
-            state.hBest = 0;
+            state.hBest = PathCost();
             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;
-        }
+        PathCost g = state.tiles->get(i, j).GetCost();
 
-        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);
+        // Try all 8 neighbors
+        ProcessNeighbour(i, j, i-1, j-1, g, state);
+        ProcessNeighbour(i, j, i+1, j-1, g, state);
+        ProcessNeighbour(i, j, i-1, j+1, g, state);
+        ProcessNeighbour(i, j, i+1, j+1, g, state);
+        ProcessNeighbour(i, j, i-1, j, g, state);
+        ProcessNeighbour(i, j, i+1, j, g, state);
+        ProcessNeighbour(i, j, i, j-1, g, state);
+        ProcessNeighbour(i, j, i, j+1, g, state);
     }
 
     // Reconstruct the path (in reverse)
@@ -477 +454 @@
     {
         PathfindTile& n = state.tiles->get(ip, jp);
         entity_pos_t x, z;
-        TileCenter(ip, jp, x, z);
+        NavcellCenter(ip, jp, x, z);
         Waypoint w = { x, z };
         path.m_Waypoints.push_back(w);
 
@@ -486 +463 @@
         jp = n.GetPredJ(jp);
     }
 
+    NormalizePathWaypoints(path);
+
     // Save this grid for debug display
+    m_DebugTime = timer_Time() - time;
     delete m_DebugGrid;
     m_DebugGrid = state.tiles;
     m_DebugSteps = state.steps;
+    m_DebugGoal = goal;
 
     PROFILE2_ATTR("from: (%d, %d)", i0, j0);
     PROFILE2_ATTR("to: (%d, %d)", state.iGoal, state.jGoal);
@@ -497 +478 @@
     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);
+    debug_printf(L"PATHFINDER: steps=%d avgo=%d proc=%d impc=%d impo=%d addo=%d\n", state.steps, state.sumOpenSize/state.steps, state.numProcessed, state.numImproveClosed, state.numImproveOpen, state.numAddToOpen);
 #endif
 }
+
+void CCmpPathfinder::GetDebugData(u32& steps, double& time, Grid<u8>& grid)
+{
+    steps = m_DebugSteps;
+    time = m_DebugTime;
+
+    if (!m_DebugGrid)
+        return;
+
+    grid = Grid<u8>(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)
+        {
+            PathfindTile t = m_DebugGrid->get(i, j);
+            grid.set(i, j, (t.IsOpen() ? 1 : 0) | (t.IsClosed() ? 2 : 0));
+        }
+    }
+}

source/simulation2/components/CCmpPathfinder_JPS.cpp

@@ +1 @@
+/* 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "precompiled.h"
+
+#include "CCmpPathfinder_Common.h"
+
+#include "lib/bits.h"
+#include "lib/sysdep/rtl.h"
+#include "ps/Profile.h"
+#include "renderer/TerrainOverlay.h"
+#include "simulation2/helpers/PriorityQueue.h"
+
+#define PATHFIND_STATS 1
+
+#define USE_JUMPPOINT_CACHE 1
+
+#define ACCEPT_DIAGONAL_GAPS 0
+
+typedef PriorityQueueHeap<TileID, PathCost> PriorityQueue;
+
+/**
+ * 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<u16> 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<Interval> 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);
+            Interval iv(x0, x1, obstruction);
+            data.push_back(iv);
+        }
+
+        /**
+         * 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<Interval>& 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<Interval> 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<Row> m_JumpPointsRight;
+    std::vector<Row> m_JumpPointsLeft;
+    std::vector<Row> m_JumpPointsUp;
+    std::vector<Row> 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<Row>& rows,
+        const Grid<NavcellData>& terrain, ICmpPathfinder::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.push_back(Row(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();
+        }
+    }
+
+    void reset(const Grid<NavcellData>* terrain, ICmpPathfinder::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;
+    }
+};
+
+/**
+ * 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 PathfindTileJPS
+{
+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
+    int GetPredI(int i) { return i + GetPredDI(); }
+    int GetPredJ(int j) { return j + GetPredDJ(); }
+
+    PathCost GetCost() const { return g; }
+    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:
+    u8 GetStatus() const
+    {
+        return data & 3;
+    }
+
+    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
+    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 PathfinderStateJPS
+{
+    u32 steps; // number of algorithm iterations
+
+    PathGoal goal;
+
+    u16 iGoal, jGoal; // goal tile
+
+    ICmpPathfinder::pass_class_t passClass;
+
+    PriorityQueue open;
+    // (there's no explicit closed list; it's encoded in PathfindTile)
+
+    PathfindTileJPSGrid* tiles;
+    Grid<NavcellData>* terrain;
+
+    PathCost hBest; // heuristic of closest discovered tile to goal
+    u16 iBest, jBest; // closest tile
+
+    JumpPointCache* jpc;
+
+#if PATHFIND_STATS
+    // Performance debug counters
+    size_t numProcessed;
+    size_t numImproveOpen;
+    size_t numAddToOpen;
+    size_t sumOpenSize;
+#endif
+};
+
+// Calculate heuristic cost from tile i,j to goal
+// (This ought to be an underestimate for correctness)
+static PathCost 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.
+// TODO: it'd be nice to not duplicate so much code with CCmpPathfinder_Tile.cpp
+static void ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderStateJPS& state)
+{
+#if PATHFIND_STATS
+    state.numProcessed++;
+#endif
+
+    // Reject impassable tiles
+    if (!IS_PASSABLE(state.terrain->get(i, j), state.passClass))
+        return;
+
+    PathfindTileJPS& 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);
+#if PATHFIND_STATS
+            state.numImproveOpen++;
+#endif
+            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 };
+    state.open.push(t);
+#if PATHFIND_STATS
+    state.numAddToOpen++;
+#endif
+}
+
+#define PASSABLE(i, j) IS_PASSABLE(state.terrain->get(i, j), state.passClass)
+
+/*
+ * 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.
+ */
+
+#if USE_JUMPPOINT_CACHE
+
+// Use the jump-point cache to find the jump points:
+
+static void AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderStateJPS& state)
+{
+    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);
+}
+
+static bool HasJumpedHoriz(int i, int j, int di, PathfinderStateJPS& state)
+{
+    int jump;
+    if (di > 0)
+        jump = state.jpc->GetJumpPointRight(i, j, state.goal);
+    else
+        jump = state.jpc->GetJumpPointLeft(i, j, state.goal);
+
+    return (jump != i);
+}
+
+static void AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderStateJPS& state)
+{
+    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);
+}
+
+static bool HasJumpedVert(int i, int j, int dj, PathfinderStateJPS& state)
+{
+    int jump;
+    if (dj > 0)
+        jump = state.jpc->GetJumpPointUp(i, j, state.goal);
+    else
+        jump = state.jpc->GetJumpPointDown(i, j, state.goal);
+
+    return (jump != j);
+}
+
+#else // USE_JUMPPOINT_CACHE
+
+// Find the jump points by scanning along the map:
+
+static void AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderStateJPS& state)
+{
+    ASSERT(di == 1 || di == -1);
+    int ni = i + di;
+    while (true)
+    {
+        if (!PASSABLE(ni, j))
+            break;
+
+        if ((ni == state.iGoal && j == state.jGoal) || // XXX
+#if ACCEPT_DIAGONAL_GAPS
+            (!PASSABLE(ni, j-1) && PASSABLE(ni+di, j-1)) ||
+            (!PASSABLE(ni, j+1) && PASSABLE(ni+di, j+1)))
+#else
+            (!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;
+    }
+}
+
+static bool HasJumpedHoriz(int i, int j, int di, PathfinderStateJPS& state)
+{
+    ASSERT(di == 1 || di == -1);
+    int ni = i + di;
+    while (true)
+    {
+        if (!PASSABLE(ni, j))
+            return false;
+
+        if ((ni == state.iGoal && j == state.jGoal) || // XXX
+#if ACCEPT_DIAGONAL_GAPS
+            (!PASSABLE(ni, j-1) && PASSABLE(ni+di, j-1)) ||
+            (!PASSABLE(ni, j+1) && PASSABLE(ni+di, j+1)))
+#else
+            (!PASSABLE(ni-di, j-1) && PASSABLE(ni, j-1)) ||
+            (!PASSABLE(ni-di, j+1) && PASSABLE(ni, j+1)))
+#endif
+        {
+            return true;
+        }
+
+        ni += di;
+    }
+}
+
+static void AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderStateJPS& state)
+{
+    ASSERT(dj == 1 || dj == -1);
+    int nj = j + dj;
+    while (true)
+    {
+        if (!PASSABLE(i, nj))
+            break;
+
+        if ((i == state.iGoal && nj == state.jGoal) ||
+#if ACCEPT_DIAGONAL_GAPS
+            (!PASSABLE(i-1, nj) && PASSABLE(i-1, nj+dj)) ||
+            (!PASSABLE(i+1, nj) && PASSABLE(i+1, nj+dj)))
+#else
+            (!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;
+    }
+}
+
+static bool HasJumpedVert(int i, int j, int dj, PathfinderStateJPS& state)
+{
+    ASSERT(dj == 1 || dj == -1);
+    int nj = j + dj;
+    while (true)
+    {
+        if (!PASSABLE(i, nj))
+            return false;
+
+        if ((i == state.iGoal && nj == state.jGoal) ||
+#if ACCEPT_DIAGONAL_GAPS
+            (!PASSABLE(i-1, nj) && PASSABLE(i-1, nj+dj)) ||
+            (!PASSABLE(i+1, nj) && PASSABLE(i+1, nj+dj)))
+#else
+            (!PASSABLE(i-1, nj-dj) && PASSABLE(i-1, nj)) ||
+            (!PASSABLE(i+1, nj-dj) && PASSABLE(i+1, nj)))
+#endif
+        {
+            return true;
+        }
+
+        nj += dj;
+    }
+}
+
+#endif // USE_JUMPPOINT_CACHE
+
+
+/*
+ * We don't 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.
+ */
+static void AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderStateJPS& 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;
+    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 (state.goal.NavcellContainsGoal(ni, nj))
+        {
+            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
+
+        if (HasJumpedHoriz(ni, nj, di, state) || HasJumpedVert(ni, nj, dj, state))
+        {
+            ProcessNeighbour(i, j, ni, nj, g, state);
+            return;
+        }
+
+        ni += di;
+        nj += dj;
+    }
+}
+
+
+void CCmpPathfinder::PathfinderJPSMakeDirty()
+{
+    m_JumpPointCache.clear();
+}
+
+void CCmpPathfinder::ComputePathJPS(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, Path& path)
+{
+    UpdateGrid();
+
+    PathfinderStateJPS state = { 0 };
+
+    state.jpc = m_JumpPointCache[passClass].get();
+#if USE_JUMPPOINT_CACHE
+    if (!state.jpc)
+    {
+        state.jpc = new JumpPointCache;
+        state.jpc->reset(m_Grid, passClass);
+        debug_printf(L"PATHFINDER: JPC memory: %d kB\n", (int)state.jpc->GetMemoryUsage() / 1024);
+        m_JumpPointCache[passClass] = shared_ptr<JumpPointCache>(state.jpc);
+    }
+#endif
+
+    PROFILE3("ComputePathJPS");
+    TIMER(L"ComputePathJPS");
+    double time = timer_Time();
+
+    // Convert the start coordinates to tile indexes
+    u16 i0, j0;
+    NearestNavcell(x0, z0, i0, j0);
+
+    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
+        ComputePathOffImpassable(i0, j0, passClass, path);
+        return;
+    }
+
+    state.goal = origGoal;
+    PathfinderHierMakeGoalReachable(i0, j0, state.goal, passClass);
+
+    // If we're already at the goal tile, then move directly to the exact goal coordinates
+    // XXX: this seems bogus for non-point goals, it should be the point on the current cell nearest the goal
+    if (state.goal.NavcellContainsGoal(i0, j0))
+    {
+        Waypoint w = { state.goal.x, state.goal.z };
+        path.m_Waypoints.push_back(w);
+        return;
+    }
+
+    NearestNavcell(state.goal.x, state.goal.z, state.iGoal, state.jGoal);
+
+    state.passClass = passClass;
+
+    state.steps = 0;
+
+    state.tiles = new PathfindTileJPSGrid(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 (1)
+    {
+        ++state.steps;
+
+        // 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.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;
+        }
+
+        PathfindTileJPS 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);
+            }
+            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);
+            }
+#endif
+            AddJumpedHoriz(i, j, -dpi, g, state);
+        }
+        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);
+            }
+            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);
+            }
+#endif
+            AddJumpedVert(i, j, -dpj, g, state);
+        }
+        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);
+            AddJumpedVert(i, j, -dpj, g, state);
+            AddJumpedDiag(i, j, -dpi, -dpj, g, state);
+        }
+        else
+        {
+            // No predecessor, i.e. the start tile
+            // Start searching in every direction
+
+            // XXX - check passability?
+
+            ProcessNeighbour(i, j, (u16)(i-1), (u16)(j-1), g, state);
+            ProcessNeighbour(i, j, (u16)(i+1), (u16)(j-1), g, state);
+            ProcessNeighbour(i, j, (u16)(i-1), (u16)(j+1), g, state);
+            ProcessNeighbour(i, j, (u16)(i+1), (u16)(j+1), g, state);
+            ProcessNeighbour(i, j, (u16)(i-1), j, g, state);
+            ProcessNeighbour(i, j, (u16)(i+1), j, g, state);
+            ProcessNeighbour(i, j, i, (u16)(j-1), g, state);
+            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)
+    {
+        PathfindTileJPS& n = state.tiles->get(ip, jp);
+        entity_pos_t x, z;
+        NavcellCenter(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);
+    }
+
+    NormalizePathWaypoints(path);
+
+    // Save this grid for debug display
+    m_DebugTime = timer_Time() - time;
+    delete m_DebugGridJPS;
+    m_DebugGridJPS = state.tiles;
+    m_DebugSteps = state.steps;
+    m_DebugGoal = state.goal;
+
+    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: %d", state.steps);
+
+#if PATHFIND_STATS
+    debug_printf(L"PATHFINDER: steps=%d avgo=%d proc=%d impo=%d addo=%d\n", state.steps, state.sumOpenSize/state.steps, state.numProcessed, state.numImproveOpen, state.numAddToOpen);
+#endif
+}
+
+void CCmpPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid)
+{
+    steps = m_DebugSteps;
+    time = m_DebugTime;
+
+    if (!m_DebugGridJPS)
+        return;
+
+    u16 iGoal, jGoal;
+    NearestNavcell(m_DebugGoal.x, m_DebugGoal.z, iGoal, jGoal);
+
+    grid = Grid<u8>(m_DebugGridJPS->m_W, m_DebugGridJPS->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;
+            PathfindTileJPS t = m_DebugGridJPS->get(i, j);
+            grid.set(i, j, (t.IsOpen() ? 1 : 0) | (t.IsClosed() ? 2 : 0));
+        }
+    }
+}

source/simulation2/components/CCmpPathfinder_Hier.cpp

@@ +1 @@
+/* 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "precompiled.h"
+
+#include "CCmpPathfinder_Common.h"
+
+#include "renderer/Scene.h"
+#include "renderer/TerrainOverlay.h"
+#include "simulation2/helpers/Render.h"
+
+class PathfinderHierOverlay;
+
+/**
+ * Hierarchical pathfinder.
+ * 
+ * Currently this doesn't actually find shortest paths, it just deals with
+ * connectivity.
+ *
+ * The navcell-grid representation of the map is split into fixed-size chunks.
+ * Within a chunks, 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 CCmpPathfinder_Hier
+{
+    NONCOPYABLE(CCmpPathfinder_Hier);
+
+    typedef ICmpPathfinder::pass_class_t pass_class_t;
+
+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;
+        }
+    };
+
+    CCmpPathfinder_Hier(CCmpPathfinder& pathfinder);
+    ~CCmpPathfinder_Hier();
+
+    void Init(const std::vector<PathfinderPassability>& passClasses, Grid<NavcellData>* grid);
+
+    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 i0, @p j0 (which is assumed to be an impassable navcell)
+     * to the nearest passable navcell.
+     */
+    void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass);
+
+private:
+    static const u8 CHUNK_SIZE = 64; // number of navcells per side
+
+    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<NavcellData>* grid, pass_class_t passClass);
+
+        RegionID Get(int i, int j);
+
+        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;
+
+        void FloodFill(int i0, int j0, u16 r);
+    };
+
+    typedef std::map<RegionID, std::set<RegionID> > EdgesMap;
+
+    void FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges);
+
+    void AddDebugEdges(pass_class_t passClass);
+
+    void FindReachableRegions(RegionID from, std::set<RegionID>& reachable, pass_class_t passClass);
+
+    void FindPassableRegions(std::set<RegionID>& 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<RegionID>& 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);
+    }
+
+    PathfinderHierOverlay* m_DebugOverlay;
+
+    u16 m_ChunksW, m_ChunksH;
+    std::map<pass_class_t, std::vector<Chunk> > m_Chunks;
+    
+    std::map<pass_class_t, EdgesMap> m_Edges;
+
+public:
+    CCmpPathfinder& m_Pathfinder;
+    std::vector<SOverlayLine> m_DebugOverlayLines;
+};
+
+class PathfinderHierOverlay : public TerrainTextureOverlay
+{
+public:
+    CCmpPathfinder_Hier& m_PathfinderHier;
+
+    PathfinderHierOverlay(CCmpPathfinder_Hier& pathfinderHier) :
+        TerrainTextureOverlay(ICmpObstructionManager::NAVCELLS_PER_TILE), m_PathfinderHier(pathfinderHier)
+    {
+    }
+
+    virtual void BuildTextureRGBA(u8* data, size_t w, size_t h)
+    {
+        ICmpPathfinder::pass_class_t passClass = m_PathfinderHier.m_Pathfinder.GetPassabilityClass("default");
+
+        for (size_t j = 0; j < h; ++j)
+        {
+            for (size_t i = 0; i < w; ++i)
+            {
+                SColor4ub color;
+
+                CCmpPathfinder_Hier::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;
+            }
+        }
+    }
+};
+
+void CCmpPathfinder_Hier::Chunk::InitRegions(int ci, int cj, Grid<NavcellData>* 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);
+
+    // Start by filling the grid with 0 for blocked,
+    // and a 0xFFFF placeholder for unblocked
+    for (int j = j0; j < j1; ++j)
+    {
+        for (int i = i0; i < i1; ++i)
+        {
+            if (IS_PASSABLE(grid->get(i, j), passClass))
+                m_Regions[j-j0][i-i0] = 0xFFFF;
+            else
+                m_Regions[j-j0][i-i0] = 0;
+        }
+    }
+
+    // Scan for tiles with the 0xFFFF placeholder, and then floodfill
+    // the new region this tile belongs to
+    int r = 0;
+    for (int j = 0; j < CHUNK_SIZE; ++j)
+    {
+        for (int i = 0; i < CHUNK_SIZE; ++i)
+        {
+            if (m_Regions[j][i] == 0xFFFF)
+                FloodFill(i, j, ++r);
+        }
+    }
+
+    m_NumRegions = r;
+}
+
+/**
+ * Flood-fill a connected area of navcells that are currently set to
+ * region 0xFFFF, starting at chunk-local coords (i0,j0),
+ * and assign them to region r.
+ */
+void CCmpPathfinder_Hier::Chunk::FloodFill(int i0, int j0, u16 r)
+{
+    std::vector<std::pair<u8, u8> > stack;
+    stack.push_back(std::make_pair(i0, j0));
+
+    while (!stack.empty())
+    {
+        int i = stack.back().first;
+        int j = stack.back().second;
+        stack.pop_back();
+        m_Regions[j][i] = r;
+
+        if (i > 0 && m_Regions[j][i-1] == 0xFFFF)
+            stack.push_back(std::make_pair(i-1, j));
+        if (j > 0 && m_Regions[j-1][i] == 0xFFFF)
+            stack.push_back(std::make_pair(i, j-1));
+        if (i < CHUNK_SIZE-1 && m_Regions[j][i+1] == 0xFFFF)
+            stack.push_back(std::make_pair(i+1, j));
+        if (j < CHUNK_SIZE-1 && m_Regions[j+1][i] == 0xFFFF)
+            stack.push_back(std::make_pair(i, j+1));
+    }
+}
+
+/**
+ * Returns a RegionID for the given global navcell coords
+ * (which must be inside this chunk);
+ */
+CCmpPathfinder_Hier::RegionID CCmpPathfinder_Hier::Chunk::Get(int i, int j)
+{
+    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 CCmpPathfinder_Hier::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 CCmpPathfinder_Hier::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 CCmpPathfinder_Hier::Chunk::RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const
+{
+    iBest = 0;
+    jBest = 0;
+    dist2Best = std::numeric_limits<u32>::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;
+                }
+            }
+        }
+    }
+}
+
+CCmpPathfinder_Hier::CCmpPathfinder_Hier(CCmpPathfinder& pathfinder) :
+    m_Pathfinder(pathfinder)
+{
+    m_DebugOverlay = NULL;
+//  m_DebugOverlay = new PathfinderHierOverlay(*this);
+}
+
+CCmpPathfinder_Hier::~CCmpPathfinder_Hier()
+{
+    SAFE_DELETE(m_DebugOverlay);
+}
+
+void CCmpPathfinder_Hier::Init(const std::vector<PathfinderPassability>& passClasses, Grid<NavcellData>* grid)
+{
+    PROFILE3("hier init");
+
+    // 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 (size_t n = 0; n < passClasses.size(); ++n)
+    {
+        pass_class_t passClass = passClasses[n].m_Mask;
+
+        // 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(m_Pathfinder.GetPassabilityClass("default"));
+    }
+}
+
+/**
+ * Find edges between regions in this chunk and the adjacent below/left chunks.
+ */
+void CCmpPathfinder_Hier::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)
+{
+    std::vector<Chunk>& 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 CCmpPathfinder_Hier::AddDebugEdges(pass_class_t passClass)
+{
+    const EdgesMap& edges = m_Edges[passClass];
+    const std::vector<Chunk>& chunks = m_Chunks[passClass];
+
+    for (EdgesMap::const_iterator it = edges.begin(); it != edges.end(); ++it)
+    {
+        for (std::set<RegionID>::const_iterator rit = it->second.begin(); rit != it->second.end(); ++rit)
+        {
+            // Draw a line between the two regions' centers
+
+            int i0, j0, i1, j1;
+            chunks[it->first.cj * m_ChunksW + it->first.ci].RegionCenter(it->first.r, i0, j0);
+            chunks[rit->cj * m_ChunksW + rit->ci].RegionCenter(rit->r, i1, j1);
+
+            CFixedVector2D a, b;
+            m_Pathfinder.NavcellCenter(i0, j0, a.X, a.Y);
+            m_Pathfinder.NavcellCenter(i1, j1, b.X, b.Y);
+
+            // Push the endpoints inwards a little to avoid overlaps
+            CFixedVector2D d = b - a;
+            d.Normalize(entity_pos_t::FromInt(1));
+            a += d;
+            b -= d;
+
+            std::vector<float> xz;
+            xz.push_back(a.X.ToFloat());
+            xz.push_back(a.Y.ToFloat());
+            xz.push_back(b.X.ToFloat());
+            xz.push_back(b.Y.ToFloat());
+
+            m_DebugOverlayLines.push_back(SOverlayLine());
+            m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0);
+            SimRender::ConstructLineOnGround(m_Pathfinder.GetSimContext(), xz, m_DebugOverlayLines.back(), true);
+        }
+    }
+}
+
+CCmpPathfinder_Hier::RegionID CCmpPathfinder_Hier::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 CCmpPathfinder_Hier::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass)
+{
+    RegionID source = Get(i0, j0, passClass);
+
+    // Find everywhere that's reachable
+    std::set<RegionID> reachableRegions;
+    FindReachableRegions(source, reachableRegions, passClass);
+
+//  debug_printf(L"\nReachable from (%d,%d): ", i0, j0);
+//  for (std::set<RegionID>::iterator it = reachableRegions.begin(); it != reachableRegions.end(); ++it)
+//      debug_printf(L"[%d,%d,%d], ", it->ci, it->cj, it->r);
+//  debug_printf(L"\n");
+
+    // Check whether any reachable region contains the goal
+    for (std::set<RegionID>::const_iterator it = reachableRegions.begin(); it != reachableRegions.end(); ++it)
+    {
+        // Skip region if its chunk doesn't contain the goal area
+        entity_pos_t x0 = ICmpObstructionManager::NAVCELL_SIZE * (it->ci * CHUNK_SIZE);
+        entity_pos_t z0 = ICmpObstructionManager::NAVCELL_SIZE * (it->cj * CHUNK_SIZE);
+        entity_pos_t x1 = x0 + ICmpObstructionManager::NAVCELL_SIZE * CHUNK_SIZE;
+        entity_pos_t z1 = z0 + ICmpObstructionManager::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(it->ci, it->cj, passClass).RegionContainsGoal(it->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;
+    m_Pathfinder.NearestNavcell(goal.x, goal.z, iGoal, jGoal);
+
+    FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass);
+
+    // Construct a new point goal at the nearest reachable navcell
+    PathGoal newGoal;
+    newGoal.type = PathGoal::POINT;
+    m_Pathfinder.NavcellCenter(iGoal, jGoal, newGoal.x, newGoal.z);
+    goal = newGoal;
+
+    return true;
+}
+
+void CCmpPathfinder_Hier::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass)
+{
+    std::set<RegionID> regions;
+    FindPassableRegions(regions, passClass);
+    FindNearestNavcellInRegions(regions, i, j, passClass);
+}
+
+void CCmpPathfinder_Hier::FindNearestNavcellInRegions(const std::set<RegionID>& 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<std::pair<u32, RegionID> > regionDistEsts; // pair of (distance^2, region)
+
+    for (std::set<RegionID>::const_iterator it = regions.begin(); it != regions.end(); ++it)
+    {
+        int i0 = it->ci * CHUNK_SIZE;
+        int j0 = it->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.push_back(std::make_pair(dist2, *it));
+    }
+
+    // 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<u32>::max();
+
+    for (size_t n = 0; n < regionDistEsts.size(); ++n)
+    {
+        RegionID region = regionDistEsts[n].second;
+
+        if (regionDistEsts[n].first >= dist2Best)
+            break;
+
+        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 CCmpPathfinder_Hier::FindReachableRegions(RegionID from, std::set<RegionID>& reachable, pass_class_t passClass)
+{
+    // Flood-fill the region graph, starting at 'from',
+    // collecting all the regions that are reachable via edges
+
+    std::vector<RegionID> open;
+    open.push_back(from);
+    reachable.insert(from);
+
+    while (!open.empty())
+    {
+        RegionID curr = open.back();
+        open.pop_back();
+
+        const std::set<RegionID>& neighbours = m_Edges[passClass][curr];
+        for (std::set<RegionID>::const_iterator it = neighbours.begin(); it != neighbours.end(); ++it)
+        {
+            // 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(*it).second)
+                open.push_back(*it);
+        }
+    }
+}
+
+void CCmpPathfinder_Hier::FindPassableRegions(std::set<RegionID>& regions, pass_class_t passClass)
+{
+    // Construct a set of all regions of all chunks for this pass class
+
+    const std::vector<Chunk>& chunks = m_Chunks[passClass];
+    for (size_t c = 0; c < chunks.size(); ++c)
+    {
+        // Skip the impassable region r=0
+        for (int r = 1; r < chunks[c].m_NumRegions; ++r)
+            regions.insert(RegionID(chunks[c].m_ChunkI, chunks[c].m_ChunkJ, r));
+    }
+}
+
+void CCmpPathfinder::PathfinderHierInit()
+{
+    m_PathfinderHier = new CCmpPathfinder_Hier(*this);
+}
+
+void CCmpPathfinder::PathfinderHierDeinit()
+{
+    SAFE_DELETE(m_PathfinderHier);
+}
+
+void CCmpPathfinder::PathfinderHierReload()
+{
+    m_PathfinderHier->Init(m_PassClasses, m_Grid);
+}
+
+void CCmpPathfinder::PathfinderHierRenderSubmit(SceneCollector& collector)
+{
+    for (size_t i = 0; i < m_PathfinderHier->m_DebugOverlayLines.size(); ++i)
+        collector.Submit(&m_PathfinderHier->m_DebugOverlayLines[i]);
+}
+
+bool CCmpPathfinder::PathfinderHierMakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass)
+{
+    return m_PathfinderHier->MakeGoalReachable(i0, j0, goal, passClass);
+}
+
+void CCmpPathfinder::PathfinderHierFindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass)
+{
+    m_PathfinderHier->FindNearestPassableNavcell(i, j, passClass);
+}
+ No newline at end of file

source/simulation2/components/CCmpPathfinder_Common.h

@@ -34 +34 @@
 #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"
 
 class PathfinderOverlay;
 class SceneCollector;
 struct PathfindTile;
+struct PathfindTileJPS;
+class JumpPointCache;
+class CCmpPathfinder_Hier;
 
 #ifdef NDEBUG
 #define PATHFIND_DEBUG 0
@@ -47 +52 @@
 #define PATHFIND_DEBUG 1
 #endif
 
+#define PATHFIND_USE_JPS 1
+
 /*
  * 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
@@ -64 +71 @@
  * 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
 {
@@ -107 +103 @@
         else
             m_MaxShore = std::numeric_limits<fixed>::max();
 
+        if (node.GetChild("Clearance").IsOk())
+        {
+            m_HasClearance = true;
+            m_Clearance = node.GetChild("Clearance").ToFixed();
+
+            if (!(m_Clearance % ICmpObstructionManager::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(L"Pathfinder passability class has clearance %f, should be multiple of %f",
+                    m_Clearance.ToFloat(), ICmpObstructionManager::NAVCELL_SIZE.ToFloat());
+            }
+        }
+        else
+        {
+            m_HasClearance = false;
+            m_Clearance = fixed::Zero();
+        }
     }
 
     bool IsPassable(fixed waterdepth, fixed steepness, fixed shoredist)
@@ -115 +130 @@
     }
 
     ICmpPathfinder::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;
@@ -123 +142 @@
     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)
+typedef u16 NavcellData; // 1 bit per passability class (up to PASS_CLASS_BITS)
+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)))
 
-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<PathfindTile> PathfindTileGrid;
+typedef SparseGrid<PathfindTileJPS> PathfindTileJPSGrid;
 
+/**
+ * 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.
+ */
+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;
+};
+
+/**
+ * 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;
+    }
+
+    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;
+};
+
+
+
 struct AsyncLongPathRequest
 {
     u32 ticket;
     entity_pos_t x0;
     entity_pos_t z0;
-    ICmpPathfinder::Goal goal;
+    PathGoal goal;
     ICmpPathfinder::pass_class_t passClass;
-    ICmpPathfinder::cost_class_t costClass;
     entity_id_t notify;
 };
 
@@ -156 +247 @@
     entity_pos_t z0;
     entity_pos_t r;
     entity_pos_t range;
-    ICmpPathfinder::Goal goal;
+    PathGoal goal;
     ICmpPathfinder::pass_class_t passClass;
     bool avoidMovingUnits;
     entity_id_t group;
@@ -184 +275 @@
     std::map<std::string, pass_class_t> m_PassClassMasks;
     std::vector<PathfinderPassability> m_PassClasses;
 
-    std::map<std::string, cost_class_t> m_TerrainCostClassTags;
-    std::map<std::string, cost_class_t> m_UnitCostClassTags;
-    std::vector<std::vector<u32> > m_MoveCosts; // costs[unitClass][terrainClass]
-    std::vector<std::vector<fixed> > m_MoveSpeeds; // speeds[unitClass][terrainClass]
-
     // Dynamic state:
 
     std::vector<AsyncLongPathRequest> m_AsyncLongPathRequests;
@@ -199 +285 @@
     // Lazily-constructed dynamic state (not serialized):
 
     u16 m_MapSize; // tiles per side
-    Grid<TerrainTile>* m_Grid; // terrain/passability information
-    Grid<u8>* m_ObstructionGrid; // cached obstruction information (TODO: we shouldn't bother storing this, it's redundant with LSBs of m_Grid)
+    Grid<NavcellData>* m_Grid; // terrain/passability information
+    size_t m_ObstructionGridDirtyID; // dirty ID for ICmpObstructionManager::NeedUpdate
     bool m_TerrainDirty; // indicates if m_Grid has been updated since terrain changed
     
+    std::map<pass_class_t, shared_ptr<JumpPointCache> > m_JumpPointCache; // for JPS pathfinder
+
+    // Interface to the hierarchical pathfinder.
+    // (This is hidden behind proxy methods to keep the code
+    // slightly better encapsulated.)
+    CCmpPathfinder_Hier* m_PathfinderHier;
+    void PathfinderHierInit();
+    void PathfinderHierDeinit();
+    void PathfinderHierReload();
+    void PathfinderHierRenderSubmit(SceneCollector& collector);
+    bool PathfinderHierMakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass);
+    void PathfinderHierFindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass);
+
+    void PathfinderJPSMakeDirty();
+
     // 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
@@ -210 +311 @@
     // Debugging - output from last pathfind operation:
 
     PathfindTileGrid* m_DebugGrid;
+    PathfindTileJPSGrid* m_DebugGridJPS;
     u32 m_DebugSteps;
+    double m_DebugTime;
+    PathGoal m_DebugGoal;
     Path* m_DebugPath;
     PathfinderOverlay* m_DebugOverlay;
     pass_class_t m_DebugPassClass;
@@ -236 +340 @@
 
     virtual std::map<std::string, pass_class_t> GetPassabilityClasses();
 
-    virtual cost_class_t GetCostClass(const std::string& name);
+    const PathfinderPassability* GetPassabilityFromMask(pass_class_t passClass);
 
     virtual const Grid<u16>& GetPassabilityGrid();
 
-    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, Path& 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 void ComputePathJPS(entity_pos_t x0, entity_pos_t z0, const PathGoal& 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 Goal& goal, pass_class_t passClass, Path& ret);
+    /**
+     * Same kind of interface as ICmpPathfinder::ComputePath, but works when the
+     * unit is starting on an impassable navcell. Returns a path heading directly
+     * to the nearest passable navcell.
+     */
+    void ComputePathOffImpassable(u16 i0, u16 j0, pass_class_t passClass, Path& 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);
+    /**
+     * Given a path with an arbitrary collection of waypoints, updates the
+     * waypoints to be nicer. (In particular, the current implementation
+     * ensures a consistent maximum distance between adjacent waypoints.
+     * Might be nice to improve it to smooth the paths, etc.)
+     */
+    void NormalizePathWaypoints(Path& path);
 
-    virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, const Goal& goal, pass_class_t passClass, cost_class_t costClass);
+    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 PathGoal& goal, pass_class_t passClass, Path& ret);
+
+    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 PathGoal& goal, pass_class_t passClass);
+
     virtual void ResetDebugPath();
 
     virtual void SetDebugOverlay(bool enabled);
 
-    virtual fixed GetMovementSpeed(entity_pos_t x0, entity_pos_t z0, cost_class_t costClass);
+    virtual void GetDebugData(u32& steps, double& time, Grid<u8>& grid);
 
-    virtual CFixedVector2D GetNearestPointOnGoal(CFixedVector2D pos, const Goal& goal);
+    virtual void GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid);
 
+    virtual CFixedVector2D GetNearestPointOnGoal(CFixedVector2D pos, const PathGoal& goal);
+
     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 bool CheckUnitPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass);
@@ -273 +396 @@
     virtual void ProcessSameTurnMoves();
 
     /**
-     * Returns the tile containing the given position
+     * Compute the navcell indexes on the grid nearest to a given point
      */
-    void NearestTile(entity_pos_t x, entity_pos_t z, u16& i, u16& j)
+    void NearestNavcell(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);
+        i = (u16)clamp((x / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToNegInfinity(), 0, m_MapSize*ICmpObstructionManager::NAVCELLS_PER_TILE - 1);
+        j = (u16)clamp((z / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToNegInfinity(), 0, m_MapSize*ICmpObstructionManager::NAVCELLS_PER_TILE - 1);
     }
 
     /**
@@ -286 +409 @@
      */
     static 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);
     }
 
-    static fixed DistanceToGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal);
+    static void NavcellCenter(u16 i, u16 j, entity_pos_t& x, entity_pos_t& z)
+    {
+        x = entity_pos_t::FromInt(i*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE / 2);
+        z = entity_pos_t::FromInt(j*2 + 1).Multiply(ICmpObstructionManager::NAVCELL_SIZE / 2);
+    }
 
     /**
      * Regenerates the grid based on the current obstruction list, if necessary
      */
     void UpdateGrid();
 
+    Grid<u16> ComputeShoreGrid();
+
+    void ComputeTerrainPassabilityGrid(const Grid<u16>& shoreGrid);
+
     void RenderSubmit(SceneCollector& collector);
 };
 

source/simulation2/components/CCmpPathfinder.cpp

@@ -33 +33 @@
 #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
@@ -46 +47 @@
 {
     m_MapSize = 0;
     m_Grid = NULL;
-    m_ObstructionGrid = NULL;
+    m_ObstructionGridDirtyID = 0;
     m_TerrainDirty = true;
     m_NextAsyncTicket = 1;
 
     m_DebugOverlay = NULL;
     m_DebugGrid = NULL;
+    m_DebugGridJPS = NULL;
     m_DebugPath = NULL;
 
+    PathfinderHierInit();
+
     m_SameTurnMovesCount = 0;
 
     // Since this is used as a system component (not loaded from an entity template),
@@ -86 +90 @@
     {
         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<std::string> 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<std::string>::const_iterator nit = unitClassNames.begin(); nit != unitClassNames.end(); ++nit)
-        {
-            m_UnitCostClassTags[*nit] = (cost_class_t)i;
-            ++i;
-
-            std::vector<u32> costs;
-            std::vector<fixed> 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()
@@ -152 +101 @@
     SetDebugOverlay(false); // cleans up memory
     ResetDebugPath();
 
-    delete m_Grid;
-    delete m_ObstructionGrid;
+    PathfinderHierDeinit();
+
+    SAFE_DELETE(m_Grid);
 }
 
 struct SerializeLongRequest
@@ -166 +116 @@
         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);
     }
 };
@@ -235 +184 @@
 {
     for (size_t i = 0; i < m_DebugOverlayShortPathLines.size(); ++i)
         collector.Submit(&m_DebugOverlayShortPathLines[i]);
+
+    PathfinderHierRenderSubmit(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)
 {
     if (m_PassClassMasks.find(name) == m_PassClassMasks.end())
@@ -264 +205 @@
     return m_PassClassMasks;
 }
 
-ICmpPathfinder::cost_class_t CCmpPathfinder::GetCostClass(const std::string& name)
+const PathfinderPassability* CCmpPathfinder::GetPassabilityFromMask(pass_class_t passClass)
 {
-    if (m_UnitCostClassTags.find(name) == m_UnitCostClassTags.end())
-    {
-        LOGERROR(L"Invalid unit cost class name '%hs'", name.c_str());
-        return m_UnitCostClassTags["default"];
-    }
-
-    return m_UnitCostClassTags[name];
+    for (size_t i = 0; i < m_PassClasses.size(); ++i)
+        if (m_PassClasses[i].m_Mask == passClass)
+            return &m_PassClasses[i];
+    return NULL;
 }
 
-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();
-    }
-}
-
 const Grid<u16>& CCmpPathfinder::GetPassabilityGrid()
 {
     UpdateGrid();
     return *m_Grid;
 }
 
-void CCmpPathfinder::UpdateGrid()
+/**
+ * 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: might be nicer to get rounded corners by measuring clearances as
+ * Euclidean distances; currently it effectively does dist=max(dx,dy) instead.
+ */
+static void ExpandImpassableCells(Grid<u16>& grid, u16 clearance, ICmpPathfinder::pass_class_t mask)
 {
-    CmpPtr<ICmpTerrain> cmpTerrain(GetSimContext(), SYSTEM_ENTITY);
-    if (!cmpTerrain)
-        return; // error
+    PROFILE3("ExpandImpassableCells");
 
-    // If the terrain was resized then delete the old grid data
-    if (m_Grid && m_MapSize != cmpTerrain->GetTilesPerSide())
-    {
-        SAFE_DELETE(m_Grid);
-        SAFE_DELETE(m_ObstructionGrid);
-        m_TerrainDirty = true;
-    }
+    u16 w = grid.m_W;
+    u16 h = grid.m_H;
 
-    // Initialise the terrain data when first needed
-    if (!m_Grid)
+    // First expand impassable cells horizontally into a temporary 1-bit grid
+    Grid<u8> tempGrid(w, h);
+    for (u16 j = 0; j < h; ++j)
     {
-        m_MapSize = cmpTerrain->GetTilesPerSide();
-        m_Grid = new Grid<TerrainTile>(m_MapSize, m_MapSize);
-        m_ObstructionGrid = new Grid<u8>(m_MapSize, m_MapSize);
-    }
+        // New cell (i,j) is blocked if (i',j) blocked for any i-clearance <= i' <= i+clearance
 
-    CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
+        // 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;
 
-    bool obstructionsDirty = cmpObstructionManager->Rasterise(*m_ObstructionGrid);
+        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);
 
-    if (obstructionsDirty && !m_TerrainDirty)
+            // 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)
     {
-        PROFILE("UpdateGrid obstructions");
+        // New cell (i,j) is blocked if (i,j') blocked for any j-clearance <= j' <= j+clearance
 
-        // 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
+        // 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;
 
-        // TODO: if ObstructionManager::SetPassabilityCircular was called at runtime
-        // (which should probably never happen, but that's not guaranteed),
-        // then TILE_OUTOFBOUNDS will change and we can't use this fast path, but
-        // currently it'll just set obstructionsDirty and we won't notice
-
-        for (u16 j = 0; j < m_MapSize; ++j)
+        for (u16 j = 0; j < h; ++j)
         {
-            for (u16 i = 0; i < m_MapSize; ++i)
-            {
-                TerrainTile& t = m_Grid->get(i, j);
+            // Add the mask if blocked by at least one nearby cell
+            if (numBlocked)
+                grid.set(i, j, grid.get(i, j) | mask);
 
-                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;
-            }
+            // 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;
         }
-
-        ++m_Grid->m_DirtyID;
     }
-    else if (obstructionsDirty || m_TerrainDirty)
-    {
-        PROFILE("UpdateGrid full");
+}
 
-        // Obstructions or terrain changed - we need to recompute passability
-        // TODO: only bother recomputing the region that has actually changed
+Grid<u16> CCmpPathfinder::ComputeShoreGrid()
+{
+    PROFILE3("ComputeShoreGrid");
 
-        CmpPtr<ICmpWaterManager> cmpWaterManager(GetSimContext(), SYSTEM_ENTITY);
+    CmpPtr<ICmpWaterManager> cmpWaterManager(GetSimContext(), SYSTEM_ENTITY);
 
-        // TOOD: these bits should come from ICmpTerrain
-        CTerrain& terrain = GetSimContext().GetTerrain();
+    // TOOD: these bits should come from ICmpTerrain
+    CTerrain& terrain = GetSimContext().GetTerrain();
 
-        // avoid integer overflow in intermediate calculation
-        const u16 shoreMax = 32767;
-        
-        // First pass - find underwater tiles
-        Grid<bool> waterGrid(m_MapSize, m_MapSize);
-        for (u16 j = 0; j < m_MapSize; ++j)
+    // avoid integer overflow in intermediate calculation
+    const u16 shoreMax = 32767;
+
+    // First pass - find underwater tiles
+    Grid<u8> waterGrid(m_MapSize, m_MapSize);
+    for (u16 j = 0; j < m_MapSize; ++j)
+    {
+        for (u16 i = 0; i < m_MapSize; ++i)
         {
-            for (u16 i = 0; i < m_MapSize; ++i)
-            {
-                fixed x, z;
-                TileCenter(i, j, x, z);
-                
-                bool underWater = cmpWaterManager && (cmpWaterManager->GetWaterLevel(x, z) > terrain.GetExactGroundLevelFixed(x, z));
-                waterGrid.set(i, j, underWater);
-            }
+            fixed x, z;
+            TileCenter(i, j, x, z);
+
+            bool underWater = cmpWaterManager && (cmpWaterManager->GetWaterLevel(x, z) > terrain.GetExactGroundLevelFixed(x, z));
+            waterGrid.set(i, j, underWater ? 1 : 0);
         }
-        // Second pass - find shore tiles
-        Grid<u16> shoreGrid(m_MapSize, m_MapSize);
-        for (u16 j = 0; j < m_MapSize; ++j)
+    }
+
+    // Second pass - find shore tiles
+    Grid<u16> shoreGrid(m_MapSize, m_MapSize);
+    for (u16 j = 0; j < m_MapSize; ++j)
+    {
+        for (u16 i = 0; i < m_MapSize; ++i)
         {
-            for (u16 i = 0; i < m_MapSize; ++i)
+            // Find a land tile
+            if (!waterGrid.get(i, j))
             {
-                // Find a land tile
-                if (!waterGrid.get(i, j))
+                if ((i > 0 && waterGrid.get(i-1, j)) || (i > 0 && j < m_MapSize-1 && waterGrid.get(i-1, j+1)) || (i > 0 && j > 0 && waterGrid.get(i-1, j-1))
+                    || (i < m_MapSize-1 && waterGrid.get(i+1, j)) || (i < m_MapSize-1 && j < m_MapSize-1 && waterGrid.get(i+1, j+1)) || (i < m_MapSize-1 && j > 0 && waterGrid.get(i+1, j-1))
+                    || (j > 0 && waterGrid.get(i, j-1)) || (j < m_MapSize-1 && waterGrid.get(i, j+1))
+                    )
+                {   // If it's bordered by water, it's a shore tile
+                    shoreGrid.set(i, j, 0);
+                }
+                else
                 {
-                    if ((i > 0 && waterGrid.get(i-1, j)) || (i > 0 && j < m_MapSize-1 && waterGrid.get(i-1, j+1)) || (i > 0 && j > 0 && waterGrid.get(i-1, j-1))
-                        || (i < m_MapSize-1 && waterGrid.get(i+1, j)) || (i < m_MapSize-1 && j < m_MapSize-1 && waterGrid.get(i+1, j+1)) || (i < m_MapSize-1 && j > 0 && waterGrid.get(i+1, j-1))
-                        || (j > 0 && waterGrid.get(i, j-1)) || (j < m_MapSize-1 && waterGrid.get(i, j+1))
-                        )
-                    {   // If it's bordered by water, it's a shore tile
-                        shoreGrid.set(i, j, 0);
-                    }
-                    else
-                    {
-                        shoreGrid.set(i, j, shoreMax);
-                    }
+                    shoreGrid.set(i, j, shoreMax);
                 }
             }
         }
+    }
 
-        // Expand influences on land to find shore distance
-        for (u16 y = 0; y < m_MapSize; ++y)
+    // Expand influences on land to find shore distance
+    for (u16 y = 0; y < m_MapSize; ++y)
+    {
+        u16 min = shoreMax;
+        for (u16 x = 0; x < m_MapSize; ++x)
         {
-            u16 min = shoreMax;
-            for (u16 x = 0; x < m_MapSize; ++x)
+            if (!waterGrid.get(x, y))
             {
-                if (!waterGrid.get(x, y))
-                {
-                    u16 g = shoreGrid.get(x, y);
-                    if (g > min)
-                        shoreGrid.set(x, y, min);
-                    else if (g < min)
-                        min = g;
+                u16 g = shoreGrid.get(x, y);
+                if (g > min)
+                    shoreGrid.set(x, y, min);
+                else if (g < min)
+                    min = g;
 
-                    ++min;
-                }
+                ++min;
             }
-            for (u16 x = m_MapSize; x > 0; --x)
+        }
+        for (u16 x = m_MapSize; x > 0; --x)
+        {
+            if (!waterGrid.get(x-1, y))
             {
-                if (!waterGrid.get(x-1, y))
-                {
-                    u16 g = shoreGrid.get(x-1, y);
-                    if (g > min)
-                        shoreGrid.set(x-1, y, min);
-                    else if (g < min)
-                        min = g;
+                u16 g = shoreGrid.get(x-1, y);
+                if (g > min)
+                    shoreGrid.set(x-1, y, min);
+                else if (g < min)
+                    min = g;
 
-                    ++min;
-                }
+                ++min;
             }
         }
-        for (u16 x = 0; x < m_MapSize; ++x)
+    }
+    for (u16 x = 0; x < m_MapSize; ++x)
+    {
+        u16 min = shoreMax;
+        for (u16 y = 0; y < m_MapSize; ++y)
         {
-            u16 min = shoreMax;
-            for (u16 y = 0; y < m_MapSize; ++y)
+            if (!waterGrid.get(x, y))
             {
-                if (!waterGrid.get(x, y))
-                {
-                    u16 g = shoreGrid.get(x, y);
-                    if (g > min)
-                        shoreGrid.set(x, y, min);
-                    else if (g < min)
-                        min = g;
+                u16 g = shoreGrid.get(x, y);
+                if (g > min)
+                    shoreGrid.set(x, y, min);
+                else if (g < min)
+                    min = g;
 
-                    ++min;
-                }
+                ++min;
             }
-            for (u16 y = m_MapSize; y > 0; --y)
+        }
+        for (u16 y = m_MapSize; y > 0; --y)
+        {
+            if (!waterGrid.get(x, y-1))
             {
-                if (!waterGrid.get(x, y-1))
-                {
-                    u16 g = shoreGrid.get(x, y-1);
-                    if (g > min)
-                        shoreGrid.set(x, y-1, min);
-                    else if (g < min)
-                        min = g;
+                u16 g = shoreGrid.get(x, y-1);
+                if (g > min)
+                    shoreGrid.set(x, y-1, min);
+                else if (g < min)
+                    min = g;
 
-                    ++min;
-                }
+                ++min;
             }
         }
+    }
 
-        // Apply passability classes to terrain
-        for (u16 j = 0; j < m_MapSize; ++j)
+    return shoreGrid;
+}
+
+void CCmpPathfinder::ComputeTerrainPassabilityGrid(const Grid<u16>& shoreGrid)
+{
+    PROFILE3("terrain passability");
+
+    CmpPtr<ICmpWaterManager> cmpWaterManager(GetSimContext(), SYSTEM_ENTITY);
+
+    CTerrain& terrain = GetSimContext().GetTerrain();
+
+    // Compute initial terrain-dependent passability
+    for (int j = 0; j < m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE; ++j)
+    {
+        for (int i = 0; i < m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE; ++i)
         {
-            for (u16 i = 0; i < m_MapSize; ++i)
+            // World-space coordinates for this navcell
+            fixed x, z;
+            NavcellCenter(i, j, x, z);
+
+            // Terrain-tile coordinates for this navcell
+            int itile = i / ICmpObstructionManager::NAVCELLS_PER_TILE;
+            int jtile = j / ICmpObstructionManager::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)) * (int)TERRAIN_TILE_SIZE;
+
+            // Compute the passability for every class for this cell:
+
+            NavcellData t = 0;
+            for (size_t n = 0; n < m_PassClasses.size(); ++n)
             {
-                fixed x, z;
-                TileCenter(i, j, x, z);
+                if (!m_PassClasses[n].IsPassable(depth, slope, shoredist))
+                    t |= m_PassClasses[n].m_Mask;
+            }
 
-                TerrainTile t = 0;
+            m_Grid->set(i, j, t);
+        }
+    }
+}
 
-                u8 obstruct = m_ObstructionGrid->get(i, j);
+void CCmpPathfinder::UpdateGrid()
+{
+    CmpPtr<ICmpTerrain> cmpTerrain(GetSimContext(), SYSTEM_ENTITY);
+    if (!cmpTerrain)
+        return; // error
 
-                fixed height = terrain.GetExactGroundLevelFixed(x, z);
+    // If the terrain was resized then delete the old grid data
+    if (m_Grid && m_MapSize != cmpTerrain->GetTilesPerSide())
+    {
+        SAFE_DELETE(m_Grid);
+        m_TerrainDirty = true;
+    }
 
-                fixed water;
-                if (cmpWaterManager)
-                    water = cmpWaterManager->GetWaterLevel(x, z);
+    // Initialise the terrain data when first needed
+    if (!m_Grid)
+    {
+        m_MapSize = cmpTerrain->GetTilesPerSide();
+        m_Grid = new Grid<NavcellData>(m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE, m_MapSize * ICmpObstructionManager::NAVCELLS_PER_TILE);
+        m_ObstructionGridDirtyID = 0;
+    }
 
-                fixed depth = water - height;
+    CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
 
-                fixed slope = terrain.GetSlopeFixed(i, j);
+    bool obstructionsDirty = cmpObstructionManager->NeedUpdate(&m_ObstructionGridDirtyID);
 
-                fixed shoredist = fixed::FromInt(shoreGrid.get(i, j));
+    // TODO: for performance, it'd be nice if we could get away with not
+    // recomputing all the terrain passability when only an obstruction has
+    // changed. But that's not supported yet, so recompute everything after
+    // every change:
+    if (obstructionsDirty || m_TerrainDirty)
+    {
+        PROFILE3("UpdateGrid full");
 
-                if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING)
-                    t |= 1;
+        // Obstructions or terrain changed - we need to recompute passability
+        // TODO: only bother recomputing the region that has actually changed
 
-                if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_FOUNDATION)
-                    t |= 2;
+        Grid<u16> shoreGrid = ComputeShoreGrid();
 
-                if (obstruct & ICmpObstructionManager::TILE_OUTOFBOUNDS)
+        ComputeTerrainPassabilityGrid(shoreGrid);
+
+        if (1) // XXX: if circular
+        {
+            PROFILE3("off-world passability");
+
+            // WARNING: CCmpRangeManager::LosIsOffWorld needs to be kept in sync with this
+            const int edgeSize = 3 * ICmpObstructionManager::NAVCELLS_PER_TILE; // number of tiles around the edge that will be off-world
+
+            NavcellData edgeMask = 0;
+            for (size_t n = 0; n < m_PassClasses.size(); ++n)
+                edgeMask |= m_PassClasses[n].m_Mask;
+
+            int w = m_Grid->m_W;
+            int h = m_Grid->m_H;
+            for (int j = 0; j < h; ++j)
+            {
+                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);
+                }
             }
         }
 
+        // Expand the impassability grid, for any class with non-zero clearance,
+        // so that we can stop units getting too close to impassable navcells
+        for (size_t n = 0; n < m_PassClasses.size(); ++n)
+        {
+            if (m_PassClasses[n].m_HasClearance)
+            {
+                // TODO: if multiple classes have the same clearance, we should
+                // only bother doing this once for them all
+                int clearance = (m_PassClasses[n].m_Clearance / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToInfinity();
+                if (clearance > 0)
+                    ExpandImpassableCells(*m_Grid, clearance, m_PassClasses[n].m_Mask);
+            }
+        }
+
+        // Add obstructions onto the grid, for any class with (possibly zero) clearance
+        for (size_t n = 0; n < m_PassClasses.size(); ++n)
+        {
+            // TODO: if multiple classes have the same clearance, we should
+            // only bother running Rasterize once for them all
+            if (m_PassClasses[n].m_HasClearance)
+                cmpObstructionManager->Rasterize(*m_Grid, m_PassClasses[n].m_Clearance, ICmpObstructionManager::FLAG_BLOCK_PATHFINDING, m_PassClasses[n].m_Mask);
+        }
+
         m_TerrainDirty = false;
 
         ++m_Grid->m_DirtyID;
+
+        PathfinderHierReload();
+
+        PathfinderJPSMakeDirty();
     }
 }
 
@@ -542 +568 @@
 
 // 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);
@@ -580 +606 @@
     {
         const AsyncLongPathRequest& req = longRequests[i];
         Path path;
-        ComputePath(req.x0, req.z0, req.goal, req.passClass, req.costClass, path);
+#if PATHFIND_USE_JPS
+        ComputePathJPS(req.x0, req.z0, req.goal, req.passClass, path);
+#else
+        ComputePath(req.x0, req.z0, req.goal, req.passClass, path);
+#endif
         CMessagePathResult msg(req.ticket, path);
         GetSimContext().GetComponentManager().PostMessage(req.notify, msg);
     }
@@ -656 +686 @@
     }
 }
 
+//////////////////////////////////////////////////////////
+
 bool CCmpPathfinder::CheckUnitPlacement(const IObstructionTestFilter& filter,
     entity_pos_t x, entity_pos_t z, entity_pos_t r, pass_class_t passClass)
 {
@@ -667 +699 @@
     if (cmpObstructionManager->TestUnitShape(filter, x, z, r, NULL))
         return false;
 
-    // Test against terrain:
+    // Test against terrain and static obstructions:
 
     UpdateGrid();
 
-    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 false;
-            }
-        }
-    }
+    u16 i, j;
+    NearestNavcell(x, z, i, j);
+    if (!IS_PASSABLE(m_Grid->get(i, j), passClass))
+        return false;
+
+    // (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 true;
 }
 
@@ -711 +739 @@
     if (!cmpObstruction->GetObstructionSquare(square))
         return false;
 
-    // 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);
+    entity_pos_t expand;
+    const PathfinderPassability* passability = GetPassabilityFromMask(passClass);
+    if (passability && passability->m_HasClearance)
+        expand = passability->m_Clearance;
 
-    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)
+    SimRasterize::Spans spans;
+    SimRasterize::RasterizeRectWithClearance(spans, square, expand, ICmpObstructionManager::NAVCELL_SIZE);
+    for (size_t k = 0; k < spans.size(); ++k)
     {
-        for (u16 i = i0; i <= i1; ++i)
+        i16 i0 = spans[k].i0;
+        i16 i1 = spans[k].i1;
+        i16 j = spans[k].j;
+
+        // Fail if any span extends outside the grid
+        if (i0 < 0 || i1 > m_Grid->m_W || j < 0 || j > m_Grid->m_H)
+            return false;
+
+        // 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 false;
+    }
+
+    return true;
+}
+
+//////////////////////////////////////////////////////////
+
+void CCmpPathfinder::ComputePathOffImpassable(u16 i0, u16 j0, pass_class_t passClass, Path& path)
+{
+    u16 iGoal = i0;
+    u16 jGoal = j0;
+    this->PathfinderHierFindNearestPassableNavcell(iGoal, jGoal, passClass);
+
+    int ip = iGoal;
+    int jp = jGoal;
+
+    // Reconstruct the path (in reverse)
+    while (ip != i0 || jp != j0)
+    {
+        entity_pos_t x, z;
+        NavcellCenter(ip, jp, x, z);
+        Waypoint w = { x, z };
+        path.m_Waypoints.push_back(w);
+
+        // 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 CCmpPathfinder::NormalizePathWaypoints(Path& path)
+{
+    if (path.m_Waypoints.empty())
+        return;
+
+    // Given the current list of waypoints, add intermediate waypoints
+    // in a straight line between them, so that the maximum gap between
+    // waypoints is within the (fairly arbitrary) limit
+    const fixed MAX_WAYPOINT_SEPARATION = ICmpObstructionManager::NAVCELL_SIZE * 4;
+
+    std::vector<Waypoint>& waypoints = path.m_Waypoints;
+    std::vector<Waypoint> newWaypoints;
+
+    newWaypoints.push_back(waypoints.front());
+    for (size_t k = 1; k < waypoints.size(); ++k)
+    {
+        CFixedVector2D prev(waypoints[k-1].x, waypoints[k-1].z);
+        CFixedVector2D curr(waypoints[k].x, waypoints[k].z);
+        fixed dist = (curr - prev).Length();
+        if (dist > MAX_WAYPOINT_SEPARATION)
         {
-            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))
+            int segments = (dist / MAX_WAYPOINT_SEPARATION).ToInt_RoundToInfinity();
+            for (int i = 1; i < segments; ++i)
             {
-                return false;
+                CFixedVector2D p = prev + ((curr - prev)*i) / segments;
+                Waypoint wp = { p.X, p.Y };
+                newWaypoints.push_back(wp);
             }
         }
+        newWaypoints.push_back(waypoints[k]);
     }
 
-    return true;
+    path.m_Waypoints.swap(newWaypoints);
 }

source/simulation2/components/CCmpObstructionManager.cpp

@@ -22 +22 @@
 
 #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"
@@ -435 +436 @@
     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<entity_id_t>* out);
     virtual bool TestUnitShape(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, std::vector<entity_id_t>* out);
 
-    virtual bool Rasterise(Grid<u8>& grid);
+    virtual void Rasterize(Grid<u16>& grid, 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<ObstructionSquare>& 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<entity_id_t>& out);
 
     virtual void SetPassabilityCircular(bool enabled)
     {
@@ -505 +506 @@
         m_DebugOverlayDirty = true;
     }
 
-    /**
-     * Test whether a Rasterise()d grid is dirty and needs updating
-     */
-    template<typename T>
-    bool IsDirty(const Grid<T>& grid)
+    virtual bool NeedUpdate(size_t* dirtyID)
     {
-        return grid.m_DirtyID < m_DirtyID;
+        ENSURE(*dirtyID <= m_DirtyID);
+        if (*dirtyID != m_DirtyID)
+        {
+            *dirtyID = m_DirtyID;
+            return true;
+        }
+        return false;
     }
 
     /**
@@ -703 +706 @@
 }
 
 /**
- * Compute the tile indexes on the grid nearest to a given point
+ * Compute the navcell 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 NearestNavcell(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);
+    i = (u16)clamp((x / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToZero(), 0, w-1);
+    j = (u16)clamp((z / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToZero(), 0, h-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)
+void CCmpObstructionManager::Rasterize(Grid<u16>& grid, entity_pos_t expand, ICmpObstructionManager::flags_t requireMask, u16 setMask)
 {
-    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);
-}
+    PROFILE3("Rasterize");
 
-bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
-{
-    if (!IsDirty(grid))
-        return false;
+    // Since m_DirtyID is only updated for pathfinding/foundation blocking shapes,
+    // NeedUpdate+Rasterise 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)));
 
-    PROFILE("Rasterise");
+    // 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.)
 
-    grid.m_DirtyID = m_DirtyID;
+    // TODO: it might be nice to rasterize with rounded corners for large 'expand' values.
 
-    // 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
+    // (This could be implemented much more efficiently.)
 
-    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<u32, StaticShape>::iterator it = m_StaticShapes.begin(); it != m_StaticShapes.end(); ++it)
     {
-        CFixedVector2D center(it->second.x, it->second.z);
-
-        if (it->second.flags & FLAG_BLOCK_PATHFINDING)
+        const StaticShape& shape = it->second;
+        if (shape.flags & requireMask)
         {
-            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)
+            ObstructionSquare square = { shape.x, shape.z, shape.u, shape.v, shape.hw, shape.hh };
+            SimRasterize::Spans spans;
+            SimRasterize::RasterizeRectWithClearance(spans, square, expand, ICmpObstructionManager::NAVCELL_SIZE);
+            for (size_t k = 0; k < spans.size(); ++k)
             {
-                for (u16 i = i0; i <= i1; ++i)
+                i16 j = spans[k].j;
+                if (j >= 0 && j <= grid.m_H)
                 {
-                    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);
+                    i16 i0 = std::max(spans[k].i0, (i16)0);
+                    i16 i1 = std::min(spans[k].i1, (i16)grid.m_W);
+                    for (i16 i = i0; i < i1; ++i)
+                        grid.set(i, j, grid.get(i, j) | setMask);
                 }
             }
         }
-
-        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);
-                }
-            }
-        }
     }
 
     for (std::map<u32, UnitShape>::iterator it = m_UnitShapes.begin(); it != m_UnitShapes.end(); ++it)
     {
         CFixedVector2D center(it->second.x, it->second.z);
 
-        if (it->second.flags & FLAG_BLOCK_PATHFINDING)
+        if (it->second.flags & requireMask)
         {
-            entity_pos_t r = it->second.r + expandPathfinding;
+            entity_pos_t r = it->second.r + expand;
 
             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);
+            NearestNavcell(center.X - r, center.Y - r, i0, j0, grid.m_W, grid.m_H);
+            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);
         }
-
-        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);
-    }
-
-    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<ObstructionSquare>& squares)
@@ -923 +820 @@
     }
 }
 
-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<entity_id_t>& out)
 {
-    std::vector<ObstructionSquare> squares;
+    PROFILE3("GetUnitsOnObstruction");
 
-    CFixedVector2D center(x, z);
+    // Ideally we'd 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.
+    // But ObstructionManager doesn't actually know the clearance values
+    // (only the radiuses), so we can't easily do that properly.
+    // Instead, cheat and just ignore the clearance entirely;
+    // this might allow players to build buildings so a unit ends up on
+    // an impassable tile, which is slightly bad and might let players cheat,
+    // so TODO: fix this properly.
 
-    // 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<ObstructionSquare>::reverse_iterator it = squares.rbegin(); it != squares.rend(); ++it)
-    {
-        CFixedVector2D halfSize(it->hw, it->hh);
-        if (Geometry::PointIsInSquare(CFixedVector2D(it->x, it->z) - center, it->u, it->v, halfSize))
-        {
-            square = *it;
-            return true;
-        }
-    }
+    SimRasterize::Spans spans;
+    SimRasterize::RasterizeRectWithClearance(spans, square, fixed::Zero(), ICmpObstructionManager::NAVCELL_SIZE);
+ 
+    for (std::map<u32, UnitShape>::iterator it = m_UnitShapes.begin(); it != m_UnitShapes.end(); ++it)
+    {
+        // Check whether the unit's center is on a navcell that's in
+        // any of the spans
 
-    // 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<ObstructionSquare>::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;
-        }
-    }
+        u16 i = (it->second.x / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToNegInfinity();
+        u16 j = (it->second.z / ICmpObstructionManager::NAVCELL_SIZE).ToInt_RoundToNegInfinity();
 
-    return false;
+        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(it->second.entity);
+        }
+    }
+    //TODO Should we expand by r here?
 }
 
 void CCmpObstructionManager::RenderSubmit(SceneCollector& collector)

source/simulation2/components/CCmpObstruction.cpp

@@ -553 +553 @@
     {
         std::vector<entity_id_t> ret;
 
-        CmpPtr<ICmpPosition> cmpPosition(GetSimContext(), GetEntityId());
-        if (!cmpPosition)
-            return ret; // error
-
-        if (!cmpPosition->IsInWorld())
-            return ret; // no obstruction
-
-        CFixedVector2D pos = cmpPosition->GetPosition2D();
-
         CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
         if (!cmpObstructionManager)
             return ret; // error
 
-        // required precondition to use SkipControlGroupsRequireFlagObstructionFilter
-        if (m_ControlGroup == INVALID_ENTITY)
-        {
-            LOGERROR(L"[CmpObstruction] Cannot test for unit or structure obstructions; primary control group must be valid");
-            return ret;
-        }
-
         flags_t flags = 0;
         bool invertMatch = false;
 
@@ -595 +579 @@
             flags = ICmpObstructionManager::FLAG_BLOCK_FOUNDATION | ICmpObstructionManager::FLAG_BLOCK_PATHFINDING;
         }
 
-        // 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 
-        // 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);
-
-        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);
+        ICmpObstructionManager::ObstructionSquare square;
+        if (!GetObstructionSquare(square))
+            return ret; // error
+            
+        cmpObstructionManager->GetUnitsOnObstruction(square, ret);
+        //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;
     }

source/renderer/TerrainOverlay.h

@@ -26 +26 @@
 #include "lib/ogl.h"
 
 struct CColor;
+struct SColor4ub;
 class CTerrain;
 class CSimContext;
 
@@ -186 +187 @@
      */
     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();
 

source/renderer/TerrainOverlay.cpp

@@ -331 +331 @@
 
     g_Renderer.GetTerrainRenderer().RenderTerrainOverlayTexture(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);
+}

source/maths/FixedVector2D.h

@@ -79 +79 @@
         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.

source/maths/Brush.cpp

@@ -379 +379 @@
 
     ENSURE(prev == &result);
 }
+
 std::vector<CVector3D> CBrush::GetVertices() const 
 {
     return m_Vertices;

source/gui/GUIManager.h

@@ -145 +145 @@
         CStrW name;
         boost::unordered_set<VfsPath> inputs; // for hotloading
 
-        JSContext* cx;
         CScriptValRooted initData; // data to be passed to the init() function
 
         shared_ptr<CGUI> gui; // the actual GUI page

source/graphics/TerritoryTexture.cpp

@@ -25 +25 @@
 #include "ps/Profile.h"
 #include "renderer/Renderer.h"
 #include "simulation2/Simulation2.h"
+#include "simulation2/components/ICmpObstructionManager.h"
 #include "simulation2/components/ICmpPlayer.h"
 #include "simulation2/components/ICmpPlayerManager.h"
 #include "simulation2/components/ICmpTerrain.h"
@@ -92 +93 @@
     if (!cmpTerrain)
         return;
 
-    m_MapSize = cmpTerrain->GetVerticesPerSide() - 1;
+    // Convert size from terrain tiles to territory tiles
+    m_MapSize = cmpTerrain->GetTilesPerSide() * ICmpObstructionManager::NAVCELLS_PER_TILE / ICmpTerritoryManager::NAVCELLS_PER_TERRITORY_TILE;
 
     m_TextureSize = (GLsizei)round_up_to_pow2((size_t)m_MapSize);
 

source/graphics/TerritoryBoundary.cpp

@@ -21 +21 @@
 #include <algorithm> // for reverse
 
 #include "graphics/Terrain.h"
+#include "simulation2/components/ICmpObstructionManager.h"
 #include "simulation2/components/ICmpTerritoryManager.h"
 
 std::vector<STerritoryBoundary> CTerritoryBoundaryCalculator::ComputeBoundaries(const Grid<u8>* territory)
@@ -123 +124 @@
             u16 maxi = (u16)(grid.m_W-1);
             u16 maxj = (u16)(grid.m_H-1);
 
+            // Size of a territory tile in metres
+            float territoryTileSize = (ICmpObstructionManager::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

source/graphics/Terrain.h

@@ -87 +87 @@
     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;

source/graphics/Terrain.cpp

@@ -338 +338 @@
     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

binaries/data/mods/public/simulation/templates/units/hele_mechanical_siege_tower.xml

@@ -8 +8 @@
     <History>When Demetrius Poliorcetes besieged Salamis, in Cyprus, he instructed that a machine be constructed, which he called "the taker of cities." Its form was that of a square tower, each side 90 cubits high and 45 wide. It rested on four wheels, each eight cubits high. It was divided into nine stories, the lower of which contained machines for throwing great stones, the middle large catapults for throwing spears, and the highest, other machines for throwing smaller stones, together with smaller catapults. It was manned with 200 soldiers, besides those that moved it by pushing the parallel beams at the bottom.</History>
     <Icon>units/hele_mechanical_siege_tower.png</Icon>
   </Identity>
+  <Obstruction>
+    <Unit radius="3.75"/>
+  </Obstruction>
+  <UnitMotion>
+    <PassabilityClass>siege-large</PassabilityClass>
+  </UnitMotion>
   <Vision>
     <Range>88</Range>
   </Vision>

binaries/data/mods/public/simulation/templates/template_unit_mechanical_ship_fishing.xml

@@ -43 +43 @@
     </Rates>
   </ResourceGatherer>
   <UnitMotion>
+    <PassabilityClass>ship-small</PassabilityClass>
     <WalkSpeed>8.5</WalkSpeed>
   </UnitMotion>
   <Vision>

binaries/data/mods/public/simulation/templates/template_unit_infantry.xml

@@ -118 +118 @@
     <Run>
       <Speed>18.75</Speed>
     </Run>
-    <CostClass>infantry</CostClass>
   </UnitMotion>
   <Vision>
     <Range>60</Range>

binaries/data/mods/public/simulation/templates/template_unit.xml

@@ -109 +109 @@
       <DecayTime>0.2</DecayTime>
     </Run>
     <PassabilityClass>default</PassabilityClass>
-    <CostClass>default</CostClass>
   </UnitMotion>
   <Vision>
     <Range>10</Range>

binaries/data/mods/public/simulation/templates/template_structure.xml

@@ -68 +68 @@
     <LineDashColour r="158" g="11" b="15"/>
     <LineStartCap>square</LineStartCap>
     <LineEndCap>round</LineEndCap>
-    <LineCostClass>default</LineCostClass>
     <LinePassabilityClass>default</LinePassabilityClass>
   </RallyPointRenderer>
   <Selectable>

binaries/data/mods/public/simulation/templates/special/formation.xml

@@ -22 +22 @@
     <FormationController>true</FormationController>
     <WalkSpeed>1.0</WalkSpeed>
     <PassabilityClass>default</PassabilityClass>
-    <CostClass>default</CostClass>
   </UnitMotion>
 </Entity>

binaries/data/mods/public/simulation/data/pathfinder.xml

@@ -6 +6 @@
   
   <PassabilityClasses>
 
+    <unrestricted/>
+
     <!-- Unit pathfinding classes: -->
-    <unrestricted/>
     <default>
       <MaxWaterDepth>2</MaxWaterDepth>
       <MaxTerrainSlope>1.0</MaxTerrainSlope>
+      <Clearance>1.0</Clearance>
     </default>
+    <siege-large>
+      <MaxWaterDepth>2</MaxWaterDepth>
+      <MaxTerrainSlope>1.0</MaxTerrainSlope>
+      <Clearance>4.0</Clearance>
+    </siege-large>
+    <default-no-clearance>
+      <MaxWaterDepth>2</MaxWaterDepth>
+      <MaxTerrainSlope>1.0</MaxTerrainSlope>
+      <Clearance>0.0</Clearance>
+    </default-no-clearance>
     <ship>
       <MinWaterDepth>1</MinWaterDepth>
+      <Clearance>12.0</Clearance>
     </ship>
+    <ship-small>
+      <MinWaterDepth>1</MinWaterDepth>
+      <Clearance>4.0</Clearance>
+    </ship-small>
+
     <!-- Building construction classes:
     * Land is used for most buildings, which must be away
         from water and not on cliffs or mountains.
     * Shore is used for docks, which must be near water and
         land, yet shallow enough for builders to approach.
+
+      (These should not use <Clearance>, because the foundation placement
+      checker already does precise obstruction-shape-based checking.)
     -->
     <building-land>
       <MaxWaterDepth>0</MaxWaterDepth>
-      <MinShoreDistance>1.0</MinShoreDistance>
+      <MinShoreDistance>4.0</MinShoreDistance>
       <MaxTerrainSlope>1.0</MaxTerrainSlope>
     </building-land>
     <building-shore>
-      <MaxShoreDistance>2.0</MaxShoreDistance>
+      <MaxShoreDistance>8.0</MaxShoreDistance>
       <MaxTerrainSlope>1.25</MaxTerrainSlope>
     </building-shore>
 
+    <!-- Territory growth influences: -->
+    <territory>
+      <MaxWaterDepth>2</MaxWaterDepth>
+      <MaxTerrainSlope>1.0</MaxTerrainSlope>
+    </territory>
+
   </PassabilityClasses>
-  
-  <!--
-    Warning: Movement costs are a subtle tradeoff between
-    pathfinding accuracy and computation cost. Be extremely
-    careful if you change them.
-    (Speeds are safer to change, but ought to be kept roughly
-    in sync with costs.)
-  -->
-  <MovementClasses>
-    <default Speed="1.0" Cost="1.08"/>
-    <city Speed="1.0" Cost="1.0">
-      <UnitClasses>
-        <infantry Speed="1.4" Cost="0.6"/>
-      </UnitClasses>
-    </city>
-  </MovementClasses>
 </Pathfinder>

binaries/data/mods/public/art/textures/terrain/types/special/textures.xml

@@ +1 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Textures>
+    <File pattern="grid_subdiv.png" format="rgba"/>
+</Textures>

binaries/data/mods/public/art/textures/terrain/types/special/grid_subdiv.xml

@@ +1 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<Terrains>
+    <Terrain angle="0.0" size="4.0"/>
+</Terrains>

binaries/data/mods/public/art/textures/terrain/types/special/grid_subdiv.xml

@@ +1 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<Terrains>
+    <Terrain angle="0.0" size="4.0"/>
+</Terrains>

binaries/data/mods/public/art/textures/terrain/types/special/textures.xml

@@ +1 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Textures>
+    <File pattern="grid_subdiv.png" format="rgba"/>
+</Textures>