Ticket #930: pathfinder-incremental-grid-update.patch

source/simulation2/helpers/Grid.h

@@ -19 +19 @@
 #define INCLUDED_GRID
 
 #include <cstring>
+#include <vector>
 
 #ifdef NDEBUG
 #define GRID_BOUNDS_DEBUG 0
@@ -27 +28 @@
 #endif
 
 /**
+ * Can be used to observe modifications of a Grid
+ */
+class GridObserver {
+public:
+    typedef std::vector< std::pair<u16, u16> >::const_iterator const_iterator;
+
+    // Methods modifying the observer itself
+    GridObserver() : m_Reseted(false) {
+        m_Modified.reserve(3000); //FIXME 3000 ? Needs some estimation.
+    }
+    void clear() {
+        m_Modified.clear();
+        m_Reseted = false;
+    }
+
+    // Methods indicating changes on the observed grid
+    void modifiy(u16 i, u16 j) {
+        //TODO Need check to avoid doubles
+        m_Modified.push_back(std::make_pair(i, j));
+    }
+    void reset() {
+        m_Reseted = true;
+    }
+
+    // Methods to read observed modifications
+    bool reseted() const {
+        return m_Reseted;
+    }
+    const_iterator begin() const {
+        return m_Modified.begin();
+    }
+    const_iterator end() const {
+        return m_Modified.end();
+    }
+
+private:
+    std::vector< std::pair<u16, u16> > m_Modified;
+    bool m_Reseted;
+};
+
+/**
  * Basic 2D array, intended for storing tile data, plus support for lazy updates
  * by ICmpObstructionManager.
  * @c T must be a POD type that can be initialised with 0s.
@@ -35 +77 @@
 class Grid
 {
 public:
-    Grid() : m_W(0), m_H(0), m_Data(NULL), m_DirtyID(0)
+    Grid() : m_W(0), m_H(0), m_Data(NULL), m_DirtyID(0), m_Observer(NULL)
     {
     }
 
-    Grid(u16 w, u16 h) : m_W(w), m_H(h), m_Data(NULL), m_DirtyID(0)
+    Grid(u16 w, u16 h) : m_W(w), m_H(h), m_Data(NULL), m_DirtyID(0), m_Observer(NULL)
     {
         if (m_W || m_H)
             m_Data = new T[m_W * m_H];
@@ -65 +107 @@
             }
             else
                 m_Data = NULL;
+            m_Observer = NULL; //TODO determine if we have to keep the old observer (and call m_Observer->reset()), share the observer with g or set it to NULL
         }
         return *this;
     }
@@ -76 +119 @@
 
     void reset()
     {
-        if (m_Data)
+        if (m_Data) {
             memset(m_Data, 0, m_W*m_H*sizeof(T));
+            if (m_Observer)
+                m_Observer->reset();
+        }
     }
 
     void set(int i, int j, const T& value)
@@ -86 +132 @@
         ENSURE(0 <= i && i < m_W && 0 <= j && j < m_H);
 #endif
         m_Data[j*m_W + i] = value;
+        if (m_Observer)
+            m_Observer->modifiy(i, j);
     }
 
     T& get(int i, int j) const
@@ -100 +148 @@
     T* m_Data;
 
     size_t m_DirtyID; // if this is < the id maintained by ICmpObstructionManager then it needs to be updated
+    GridObserver* m_Observer;
 };
 
 /**

source/simulation2/components/CCmpPathfinder.cpp

@@ -35 +35 @@
 #include "simulation2/components/ICmpWaterManager.h"
 #include "simulation2/serialization/SerializeTemplates.h"
 
+#include "lib/timer.h"
+TIMER_ADD_CLIENT(tc_PathfinderUpdateGrid);
+
 // Default cost to move a single tile is a fairly arbitrary number, which should be big
 // enough to be precise when multiplied/divided and small enough to never overflow when
 // summing the cost of a whole path.
@@ -328 +331 @@
 
     CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
 
+    GridObserver obstructionChanges;
+    m_ObstructionGrid->m_Observer = &obstructionChanges;
+    //FIXME Hope for no throw, with throws next calls to m_ObstructionGrid::set() and reset()
+    //      will try to use the observer which no more exists
     bool obstructionsDirty = cmpObstructionManager->Rasterise(*m_ObstructionGrid);
+    m_ObstructionGrid->m_Observer = NULL;
 
     if (obstructionsDirty && !m_TerrainDirty)
     {
         PROFILE("UpdateGrid obstructions");
+        TIMER_ACCRUE(tc_PathfinderUpdateGrid);
 
         // Obstructions changed - we need to recompute passability
         // Since terrain hasn't changed we only need to update the obstruction bits
@@ -343 +352 @@
         // then TILE_OUTOFBOUNDS will change and we can't use this fast path, but
         // currently it'll just set obstructionsDirty and we won't notice
 
-        for (u16 j = 0; j < m_MapSize; ++j)
-        {
-            for (u16 i = 0; i < m_MapSize; ++i)
+        if (obstructionChanges.reseted()) {
+            for (u16 j = 0; j < m_MapSize; ++j)
             {
+                for (u16 i = 0; i < m_MapSize; ++i)
+                {
+                    TerrainTile& t = m_Grid->get(i, j);
+
+                    u8 obstruct = m_ObstructionGrid->get(i, j);
+
+                    if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_PATHFINDING)
+                        t |= 1;
+                    else
+                        t &= ~1;
+
+                    if (obstruct & ICmpObstructionManager::TILE_OBSTRUCTED_FOUNDATION)
+                        t |= 2;
+                    else
+                        t &= ~2;
+                }
+            }
+        }else {
+            GridObserver::const_iterator it  = obstructionChanges.begin();
+            GridObserver::const_iterator end = obstructionChanges.end();
+            while (it != end) {
+                u16 i = it->first;
+                u16 j = it->second;
+
+                //FIXME ugly copy/paste
                 TerrainTile& t = m_Grid->get(i, j);
 
                 u8 obstruct = m_ObstructionGrid->get(i, j);
@@ -360 +393 @@
                     t |= 2;
                 else
                     t &= (TerrainTile)~2;
+
+                ++it;
             }
         }
 
@@ -368 +403 @@
     else if (obstructionsDirty || m_TerrainDirty)
     {
         PROFILE("UpdateGrid full");
+        TIMER_ACCRUE(tc_PathfinderUpdateGrid);
 
         // Obstructions or terrain changed - we need to recompute passability
         // TODO: only bother recomputing the region that has actually changed

source/simulation2/components/CCmpObstructionManager.cpp

@@ -33 +33 @@
 #include "ps/Profile.h"
 #include "renderer/Scene.h"
 
+#include "lib/timer.h"
+TIMER_ADD_CLIENT(tc_Rasterize);
+
 // Externally, tags are opaque non-zero positive integers.
 // Internally, they are tagged (by shape) indexes into shape lists.
 // idx must be non-zero.
@@ -67 +70 @@
     ICmpObstructionManager::flags_t flags;
 };
 
+template <class S>
+class RemoveHistory {
+public:
+    typedef typename std::map<size_t, S>::const_iterator const_iterator;
+
+    RemoveHistory(size_t baseId = 1)
+    : m_HistoryBase(baseId)
+    {
+    }
+    bool isInHistory(size_t id) const {
+        return id >= m_HistoryBase;
+    }
+    void add(size_t id, S const & e) {
+        m_RemovedShapes.insert(std::make_pair(id, e));
+        if (m_RemovedShapes.size() > 10) {
+            m_HistoryBase = m_RemovedShapes.begin()->first + 1;
+            m_RemovedShapes.erase(m_RemovedShapes.begin());
+        }
+    }
+    void reset(size_t id) {
+        m_RemovedShapes.clear();
+        m_HistoryBase = id;
+    }
+    const_iterator end() const {
+        return m_RemovedShapes.end();
+    }
+    const_iterator get(size_t id) const {
+        return m_RemovedShapes.lower_bound(id);
+    }
+
+private:
+    std::map<size_t, S> m_RemovedShapes;
+    size_t m_HistoryBase;
+};
+
 /**
  * Serialization helper template for UnitShape
  */
@@ -149 +187 @@
         m_StaticShapeNext = 1;
 
         m_DirtyID = 1; // init to 1 so default-initialised grids are considered dirty
+        m_StaticRemoved.reset(m_DirtyID);
+        m_UnitRemoved.reset(m_DirtyID);
 
         m_PassabilityCircular = false;
 
@@ -250 +290 @@
         UnitShape shape = { ent, x, z, r, flags, group };
         u32 id = m_UnitShapeNext++;
         m_UnitShapes[id] = shape;
-        MakeDirtyUnit(flags);
+        MakeDirtyUnit(shape, false);
 
         m_UnitSubdivision.Add(id, CFixedVector2D(x - r, z - r), CFixedVector2D(x + r, z + r));
 
@@ -267 +307 @@
         StaticShape shape = { ent, x, z, u, v, w/2, h/2, flags };
         u32 id = m_StaticShapeNext++;
         m_StaticShapes[id] = shape;
-        MakeDirtyStatic(flags);
+        MakeDirtyStatic(shape, false);
 
         CFixedVector2D center(x, z);
         CFixedVector2D bbHalfSize = Geometry::GetHalfBoundingBox(u, v, CFixedVector2D(w/2, h/2));
@@ -302 +342 @@
         if (TAG_IS_UNIT(tag))
         {
             UnitShape& shape = m_UnitShapes[TAG_TO_INDEX(tag)];
+            
+            MakeDirtyUnit(shape, true);
 
             m_UnitSubdivision.Move(TAG_TO_INDEX(tag),
                 CFixedVector2D(shape.x - shape.r, shape.z - shape.r),
@@ -311 +353 @@
 
             shape.x = x;
             shape.z = z;
-
-            MakeDirtyUnit(shape.flags);
         }
         else
         {
@@ -323 +363 @@
 
             StaticShape& shape = m_StaticShapes[TAG_TO_INDEX(tag)];
 
+            MakeDirtyStatic(shape, true);
+
             CFixedVector2D fromBbHalfSize = Geometry::GetHalfBoundingBox(shape.u, shape.v, CFixedVector2D(shape.hw, shape.hh));
             CFixedVector2D toBbHalfSize = Geometry::GetHalfBoundingBox(u, v, CFixedVector2D(shape.hw, shape.hh));
             m_StaticSubdivision.Move(TAG_TO_INDEX(tag),
@@ -335 +377 @@
             shape.z = z;
             shape.u = u;
             shape.v = v;
-
-            MakeDirtyStatic(shape.flags);
         }
     }
 
@@ -378 +418 @@
                 CFixedVector2D(shape.x - shape.r, shape.z - shape.r),
                 CFixedVector2D(shape.x + shape.r, shape.z + shape.r));
 
-            MakeDirtyUnit(shape.flags);
+            MakeDirtyUnit(shape, true);
             m_UnitShapes.erase(TAG_TO_INDEX(tag));
         }
         else
@@ -389 +429 @@
             CFixedVector2D bbHalfSize = Geometry::GetHalfBoundingBox(shape.u, shape.v, CFixedVector2D(shape.hw, shape.hh));
             m_StaticSubdivision.Remove(TAG_TO_INDEX(tag), center - bbHalfSize, center + bbHalfSize);
 
-            MakeDirtyStatic(shape.flags);
+            MakeDirtyStatic(shape, true);
             m_StaticShapes.erase(TAG_TO_INDEX(tag));
         }
     }
@@ -444 +484 @@
     // if a grid has a lower DirtyID then it needs to be updated.
 
     size_t m_DirtyID;
+    RemoveHistory<StaticShape> m_StaticRemoved;
+    RemoveHistory<UnitShape> m_UnitRemoved;
 
+
     /**
      * Mark all previous Rasterise()d grids as dirty, and the debug display.
      * Call this when the world bounds have changed.
@@ -452 +495 @@
     void MakeDirtyAll()
     {
         ++m_DirtyID;
+        m_StaticRemoved.reset(m_DirtyID);
+        m_UnitRemoved.reset(m_DirtyID);
         m_DebugOverlayDirty = true;
     }
 
@@ -468 +513 @@
      * Mark all previous Rasterise()d grids as dirty, if they depend on this shape.
      * Call this when a static shape has changed.
      */
-    void MakeDirtyStatic(flags_t flags)
+    void MakeDirtyStatic(StaticShape const & shape, bool removed)
     {
-        if (flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION))
+        if (shape.flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION)) {
+            if (removed)
+                m_StaticRemoved.add(m_DirtyID, shape);
             ++m_DirtyID;
+        }
 
         m_DebugOverlayDirty = true;
     }
@@ -480 +528 @@
      * Mark all previous Rasterise()d grids as dirty, if they depend on this shape.
      * Call this when a unit shape has changed.
      */
-    void MakeDirtyUnit(flags_t flags)
+    void MakeDirtyUnit(UnitShape const & shape, bool removed)
     {
-        if (flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION))
+        if (shape.flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION)) {
+            if (removed)
+                m_UnitRemoved.add(m_DirtyID, shape);
             ++m_DirtyID;
+        }
 
         m_DebugOverlayDirty = true;
     }
@@ -709 +760 @@
         return false;
 
     PROFILE("Rasterise");
+    TIMER_ACCRUE(tc_Rasterize);
 
-    grid.m_DirtyID = m_DirtyID;
-
-    // TODO: this is all hopelessly inefficient
-    // What we should perhaps do is have some kind of quadtree storing Shapes so it's
-    // quick to invalidate and update small numbers of tiles
-
-    grid.reset();
-
     // For tile-based pathfinding:
     // Since we only count tiles whose centers are inside the square,
     // we maybe want to expand the square a bit so we're less likely to think there's
@@ -733 +777 @@
     // so we need to expand by at least 1/sqrt(2) of a tile
     entity_pos_t expandFoundation = (entity_pos_t::FromInt(CELL_SIZE) * 3) / 4;
 
+    entity_pos_t maxExpand = std::max(expandPathfinding, expandFoundation);
+
+    if (!m_StaticRemoved.isInHistory(grid.m_DirtyID) || !m_UnitRemoved.isInHistory(grid.m_DirtyID)) {
+        // Some pieces of history are missing, we need to reset the entire grid
+        grid.reset();
+    }else {
+        // We'll try to reset only needed tiles according to history
+
+        // Reset tiles under removed static shapes
+        RemoveHistory<StaticShape>::const_iterator srmit = m_StaticRemoved.get(grid.m_DirtyID);
+        RemoveHistory<StaticShape>::const_iterator srmend = m_StaticRemoved.end();
+        while (srmit != srmend) {
+            CFixedVector2D center(srmit->second.x, srmit->second.z);
+
+            if ((srmit->second.flags & FLAG_BLOCK_PATHFINDING) || (srmit->second.flags & FLAG_BLOCK_FOUNDATION))
+            {
+                CFixedVector2D halfSize(srmit->second.hw + maxExpand, srmit->second.hh + maxExpand);
+                CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(srmit->second.u, srmit->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, srmit->second.u, srmit->second.v, halfSize))
+                            grid.set(i, j, 0);
+                    }
+                }
+            }
+
+            ++srmit;
+        }
+
+        // Reset tiles under removed unit shapes
+        RemoveHistory<UnitShape>::const_iterator urmit = m_UnitRemoved.get(grid.m_DirtyID);
+        RemoveHistory<UnitShape>::const_iterator urmend = m_UnitRemoved.end();
+        while (urmit != urmend) {
+            CFixedVector2D center(urmit->second.x, urmit->second.z);
+
+            if ((urmit->second.flags & FLAG_BLOCK_PATHFINDING) || (urmit->second.flags & FLAG_BLOCK_FOUNDATION))
+            {
+                entity_pos_t r = urmit->second.r + maxExpand;
+
+                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, 0);
+            }
+
+            ++urmit;
+        }
+    }
+
+    grid.m_DirtyID = m_DirtyID;
+
     for (std::map<u32, StaticShape>::iterator it = m_StaticShapes.begin(); it != m_StaticShapes.end(); ++it)
     {
         CFixedVector2D center(it->second.x, it->second.z);