Ticket #930: pathfinder-incremental-grid-update-v3.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 +85 @@
 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 +115 @@
             }
             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;
     }
@@ -77 +128 @@
     void reset()
     {
         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 +141 @@
         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 +157 @@
     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_Common.h

@@ -201 +201 @@
     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)
+    GridObserver m_ObstructionChanges; // changes occured in m_ObstructionGrid when rasterise
     bool m_TerrainDirty; // indicates if m_Grid has been updated since terrain changed
     
     // For responsiveness we will process some moves in the same turn they were generated in

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_ObstructionChanges.clear();
+    m_ObstructionGrid->m_Observer = &m_ObstructionChanges;
     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 +351 @@
         // 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)
+        if (m_ObstructionChanges.reseted())
         {
-            for (u16 i = 0; i < m_MapSize; ++i)
+            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
+        {
+            for (GridObserver::const_iterator it = m_ObstructionChanges.begin(); it != m_ObstructionChanges.end(); ++it) {
+                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);
@@ -368 +401 @@
     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 ShapeHistory
+{
+public:
+    typedef typename std::map<size_t, S>::const_iterator const_iterator;
+
+    ShapeHistory(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_Modifications.insert(std::make_pair(id, e));
+        if (m_Modifications.size() > 10) {
+            m_HistoryBase = m_Modifications.begin()->first + 1;
+            m_Modifications.erase(m_Modifications.begin());
+        }
+    }
+    void reset(size_t id) 
+    {
+        m_Modifications.clear();
+        m_HistoryBase = id;
+    }
+    const_iterator end() const
+    {
+        return m_Modifications.end();
+    }
+    const_iterator get(size_t id) const
+    {
+        return m_Modifications.lower_bound(id);
+    }
+
+private:
+    std::map<size_t, S> m_Modifications;
+    size_t m_HistoryBase;
+};
+
 /**
  * Serialization helper template for UnitShape
  */
@@ -149 +192 @@
         m_StaticShapeNext = 1;
 
         m_DirtyID = 1; // init to 1 so default-initialised grids are considered dirty
+        m_StaticHistory.reset(m_DirtyID);
+        m_UnitHistory.reset(m_DirtyID);
 
+        //m_ExpandPathfinding = entity_pos_t::FromInt(CELL_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
+        m_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
+        m_ExpandFoundation = (entity_pos_t::FromInt(CELL_SIZE) * 3) / 4;
+
         m_PassabilityCircular = false;
 
         m_WorldX0 = m_WorldZ0 = m_WorldX1 = m_WorldZ1 = entity_pos_t::Zero();
@@ -250 +305 @@
         UnitShape shape = { ent, x, z, r, flags, group };
         u32 id = m_UnitShapeNext++;
         m_UnitShapes[id] = shape;
-        MakeDirtyUnit(flags);
+        MakeDirtyUnit(shape);
+        RegisterShapeTiles(shape, id);
 
         m_UnitSubdivision.Add(id, CFixedVector2D(x - r, z - r), CFixedVector2D(x + r, z + r));
 
@@ -267 +323 @@
         StaticShape shape = { ent, x, z, u, v, w/2, h/2, flags };
         u32 id = m_StaticShapeNext++;
         m_StaticShapes[id] = shape;
-        MakeDirtyStatic(flags);
+        MakeDirtyStatic(shape);
+        RegisterShapeTiles(shape, id);
 
         CFixedVector2D center(x, z);
         CFixedVector2D bbHalfSize = Geometry::GetHalfBoundingBox(u, v, CFixedVector2D(w/2, h/2));
@@ -303 +360 @@
         {
             UnitShape& shape = m_UnitShapes[TAG_TO_INDEX(tag)];
 
+            MakeDirtyUnit(shape);
+            UnregisterShapeTiles(shape, TAG_TO_INDEX(tag));
+
             m_UnitSubdivision.Move(TAG_TO_INDEX(tag),
                 CFixedVector2D(shape.x - shape.r, shape.z - shape.r),
                 CFixedVector2D(shape.x + shape.r, shape.z + shape.r),
@@ -312 +372 @@
             shape.x = x;
             shape.z = z;
 
-            MakeDirtyUnit(shape.flags);
+            MakeDirtyUnit(shape);
+            RegisterShapeTiles(shape, TAG_TO_INDEX(tag));
         }
         else
         {
@@ -323 +384 @@
 
             StaticShape& shape = m_StaticShapes[TAG_TO_INDEX(tag)];
 
+            MakeDirtyStatic(shape);
+            UnregisterShapeTiles(shape, TAG_TO_INDEX(tag));
+
             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),
@@ -336 +400 @@
             shape.u = u;
             shape.v = v;
 
-            MakeDirtyStatic(shape.flags);
+            MakeDirtyStatic(shape);
+            RegisterShapeTiles(shape, TAG_TO_INDEX(tag));
         }
     }
 
@@ -378 +443 @@
                 CFixedVector2D(shape.x - shape.r, shape.z - shape.r),
                 CFixedVector2D(shape.x + shape.r, shape.z + shape.r));
 
-            MakeDirtyUnit(shape.flags);
+            MakeDirtyUnit(shape);
+            UnregisterShapeTiles(shape, TAG_TO_INDEX(tag));
             m_UnitShapes.erase(TAG_TO_INDEX(tag));
         }
         else
@@ -389 +455 @@
             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);
+            UnregisterShapeTiles(shape, TAG_TO_INDEX(tag));
             m_StaticShapes.erase(TAG_TO_INDEX(tag));
         }
     }
@@ -444 +511 @@
     // if a grid has a lower DirtyID then it needs to be updated.
 
     size_t m_DirtyID;
+    ShapeHistory<StaticShape> m_StaticHistory;
+    ShapeHistory<UnitShape> m_UnitHistory;
 
+    // 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 m_ExpandPathfinding;
+    entity_pos_t m_ExpandFoundation;
+
+    std::multimap<u32, u32> m_StaticShapesInfluence; // key : a tile index ((i << 16) + j) ; value : index of a static shape influencing the tile
+    std::multimap<u32, u32> m_UnitShapesInfluence; // key : a tile index ((i << 16) + j) ; value : index of a unit shape influencing the tile
+
     /**
+     * Register tiles that are affected by the shape
+     */
+    void RegisterShapeTiles(StaticShape const & shape, u32 id);
+    void RegisterShapeTiles(UnitShape const & shape, u32 id);
+
+    /**
+     * Unregister tiles that were affected by the shape
+     */
+    void UnregisterShapeTiles(StaticShape const & shape, u32 id);
+    void UnregisterShapeTiles(UnitShape const & shape, u32 id);
+
+    /**
+     * Update obstruction flags of a tile
+     */
+    void UpdateTile(u16 i, u16 j, Grid<u8>& grid);
+
+    /**
+     * Update obstruction flags of tiles affected by the shape
+     */
+    void UpdateTilesUnderShape(StaticShape const & shape, Grid<u8>& grid, entity_pos_t const & expand);
+    void UpdateTilesUnderShape(UnitShape const & shape, Grid<u8>& grid, entity_pos_t const & expand);
+
+
+    /**
      * Mark all previous Rasterise()d grids as dirty, and the debug display.
      * Call this when the world bounds have changed.
      */
     void MakeDirtyAll()
     {
         ++m_DirtyID;
+        m_StaticHistory.reset(m_DirtyID);
+        m_UnitHistory.reset(m_DirtyID);
         m_DebugOverlayDirty = true;
     }
 
@@ -468 +574 @@
      * 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)
     {
-        if (flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION))
+        if (shape.flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION))
+        {
+            m_StaticHistory.add(m_DirtyID, shape);
             ++m_DirtyID;
+        }
 
         m_DebugOverlayDirty = true;
     }
@@ -480 +589 @@
      * 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)
     {
-        if (flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION))
+        if (shape.flags & (FLAG_BLOCK_PATHFINDING|FLAG_BLOCK_FOUNDATION))
+        {
+            m_UnitHistory.add(m_DirtyID, shape);
             ++m_DirtyID;
+        }
 
         m_DebugOverlayDirty = true;
     }
@@ -703 +815 @@
     z = entity_pos_t::FromInt(j*(int)CELL_SIZE + (int)CELL_SIZE/2);
 }
 
-bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
+/**
+ * Modify tiles on the shape's range
+ */
+static void ModifyTilesUnderShape(StaticShape const & shape, Grid<u8>& grid, entity_pos_t const & expand, ICmpObstructionManager::TileObstruction modificator)
 {
-    if (!IsDirty(grid))
-        return false;
+    CFixedVector2D center(shape.x, shape.z);
+    CFixedVector2D halfSize(shape.hw + expand, shape.hh + expand);
+    CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(shape.u, shape.v, halfSize);
 
-    PROFILE("Rasterise");
+    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, shape.u, shape.v, halfSize))
+                grid.set(i, j, grid.get(i, j) | modificator);
+        }
+    }
+}
+static void ModifyTilesUnderShape(UnitShape const & shape, Grid<u8>& grid, entity_pos_t const & expand, ICmpObstructionManager::TileObstruction modificator)
+{
+    CFixedVector2D center(shape.x, shape.z);
+    entity_pos_t r = shape.r + expand;
 
-    grid.m_DirtyID = m_DirtyID;
+    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) | modificator);
+}
 
-    // 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
+static bool TileIsUnderShape(u16 i, u16 j, StaticShape const & shape, Grid<u8>& grid, entity_pos_t const & expand)
+{
+    CFixedVector2D center(shape.x, shape.z);
+    CFixedVector2D halfSize(shape.hw + expand, shape.hh + expand);
+    CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(shape.u, shape.v, halfSize);
 
-    grid.reset();
+    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);
+    if (j >= j0 && j <= j1 && i >= i0 && i <= i1) {
+        entity_pos_t x, z;
+        TileCenter(i, j, x, z);
+        if (Geometry::PointIsInSquare(CFixedVector2D(x, z) - center, shape.u, shape.v, halfSize))
+            return true;
+    }
+    return false;
+}
+static bool TileIsUnderShape(u16 i, u16 j, UnitShape const & shape, Grid<u8>& grid, entity_pos_t const & expand)
+{
+    CFixedVector2D center(shape.x, shape.z);
+    entity_pos_t r = shape.r + expand;
 
-    // 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(CELL_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);
+    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);
+    return (j >= j0 && j <= j1 && i >= i0 && i <= i1);
+}
 
-    // 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(CELL_SIZE) * 3) / 4;
+void CCmpObstructionManager::RegisterShapeTiles(StaticShape const & shape, u32 id)
+{
+    if (shape.flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION))
+    {
+        entity_pos_t expand = std::max(m_ExpandPathfinding, m_ExpandFoundation);
+        CFixedVector2D center(shape.x, shape.z);
+        CFixedVector2D halfSize(shape.hw + expand, shape.hh + expand);
+        CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(shape.u, shape.v, halfSize);
 
-    for (std::map<u32, StaticShape>::iterator it = m_StaticShapes.begin(); it != m_StaticShapes.end(); ++it)
+        u16 i0 = ((center.X - halfBound.X) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 j0 = ((center.Y - halfBound.Y) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 i1 = ((center.X + halfBound.X) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 j1 = ((center.Y + halfBound.Y) / (int)CELL_SIZE).ToInt_RoundToZero();
+        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, shape.u, shape.v, halfSize))
+                {
+                    u32 key = (i << 16) + j;
+                    m_StaticShapesInfluence.insert(std::make_pair(key, id));
+                }
+            }
+        }
+    }
+}
+
+void CCmpObstructionManager::RegisterShapeTiles(UnitShape const & shape, u32 id)
+{
+    if (shape.flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION))
     {
-        CFixedVector2D center(it->second.x, it->second.z);
+        entity_pos_t expand = std::max(m_ExpandPathfinding, m_ExpandFoundation);
+        CFixedVector2D center(shape.x, shape.z);
+        entity_pos_t r = shape.r + expand;
 
-        if (it->second.flags & FLAG_BLOCK_PATHFINDING)
+        u16 i0 = ((center.X - r) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 j0 = ((center.Y - r) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 i1 = ((center.X + r) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 j1 = ((center.Y + r) / (int)CELL_SIZE).ToInt_RoundToZero();
+        for (u16 j = j0; j <= j1; ++j)
         {
-            CFixedVector2D halfSize(it->second.hw + expandPathfinding, it->second.hh + expandPathfinding);
-            CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(it->second.u, it->second.v, halfSize);
+            for (u16 i = i0; i <= i1; ++i)
+            {
+                u32 key = (i << 16) + j;
+                m_UnitShapesInfluence.insert(std::make_pair(key, id));
+            }
+        }
+    }
+}
 
-            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)
+void CCmpObstructionManager::UnregisterShapeTiles(StaticShape const & shape, u32 id)
+{
+    if (shape.flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION))
+    {
+        entity_pos_t expand = std::max(m_ExpandPathfinding, m_ExpandFoundation);
+        CFixedVector2D center(shape.x, shape.z);
+        CFixedVector2D halfSize(shape.hw + expand, shape.hh + expand);
+        CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(shape.u, shape.v, halfSize);
+
+        u16 i0 = ((center.X - halfBound.X) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 j0 = ((center.Y - halfBound.Y) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 i1 = ((center.X + halfBound.X) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 j1 = ((center.Y + halfBound.Y) / (int)CELL_SIZE).ToInt_RoundToZero();
+        for (u16 j = j0; j <= j1; ++j)
+        {
+            for (u16 i = i0; i <= i1; ++i)
             {
-                for (u16 i = i0; i <= i1; ++i)
+                entity_pos_t x, z;
+                TileCenter(i, j, x, z);
+                if (Geometry::PointIsInSquare(CFixedVector2D(x, z) - center, shape.u, shape.v, halfSize))
                 {
-                    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);
+                    typedef std::multimap<u32, u32>::iterator iterator_t;
+                    u32 key = (i << 16) + j;
+                    std::pair<iterator_t, iterator_t> range = m_StaticShapesInfluence.equal_range(key);
+                    while (range.first != range.second)
+                    {
+                        if (range.first->second == id) {
+                            iterator_t to_erase = range.first;
+                            ++range.first;
+                            m_StaticShapesInfluence.erase(to_erase);
+                        }else {
+                            ++range.first;
+                        }
+                    }
                 }
             }
         }
+    }
+}
 
-        if (it->second.flags & FLAG_BLOCK_FOUNDATION)
+void CCmpObstructionManager::UnregisterShapeTiles(UnitShape const & shape, u32 id)
+{
+    if (shape.flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION))
+    {
+        entity_pos_t expand = std::max(m_ExpandPathfinding, m_ExpandFoundation);
+        CFixedVector2D center(shape.x, shape.z);
+        entity_pos_t r = shape.r + expand;
+
+        u16 i0 = ((center.X - r) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 j0 = ((center.Y - r) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 i1 = ((center.X + r) / (int)CELL_SIZE).ToInt_RoundToZero();
+        u16 j1 = ((center.Y + r) / (int)CELL_SIZE).ToInt_RoundToZero();
+        for (u16 j = j0; j <= j1; ++j)
         {
-            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)
             {
-                for (u16 i = i0; i <= i1; ++i)
+                typedef std::multimap<u32, u32>::iterator iterator_t;
+                u32 key = (i << 16) + j;
+                std::pair<iterator_t, iterator_t> range = m_UnitShapesInfluence.equal_range(key);
+                while (range.first != range.second)
                 {
-                    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);
+                    if (range.first->second == id) {
+                        iterator_t to_erase = range.first;
+                        ++range.first;
+                        m_UnitShapesInfluence.erase(to_erase);
+                    }else {
+                        ++range.first;
+                    }
                 }
             }
         }
     }
+}
 
-    for (std::map<u32, UnitShape>::iterator it = m_UnitShapes.begin(); it != m_UnitShapes.end(); ++it)
+void CCmpObstructionManager::UpdateTile(u16 i, u16 j, Grid<uint8_t>& grid)
+{
+    typedef std::multimap<u32, u32>::const_iterator iterator_t;
+    u8 tile = 0;
+    u8 full = (TILE_OBSTRUCTED_PATHFINDING | TILE_OBSTRUCTED_FOUNDATION);
+
+    u32 key = (i << 16) + j;
+    std::pair<iterator_t, iterator_t> range = m_StaticShapesInfluence.equal_range(key);
+    while (range.first != range.second)
     {
-        CFixedVector2D center(it->second.x, it->second.z);
+        StaticShape const & shape = m_StaticShapes[range.first->second];
+        if (shape.flags & FLAG_BLOCK_PATHFINDING)
+            if (TileIsUnderShape(i, j, shape, grid, m_ExpandPathfinding))
+                tile |= TILE_OBSTRUCTED_PATHFINDING;
+        if (shape.flags & FLAG_BLOCK_FOUNDATION)
+            if (TileIsUnderShape(i, j, shape, grid, m_ExpandFoundation))
+                tile |= TILE_OBSTRUCTED_FOUNDATION;
 
-        if (it->second.flags & FLAG_BLOCK_PATHFINDING)
+        if (tile == full)
+            break;
+
+        ++range.first;
+    }
+
+    range = m_UnitShapesInfluence.equal_range(key);
+    while (tile != full && range.first != range.second)
+    {
+        UnitShape const & shape = m_UnitShapes[range.first->second];
+        if (shape.flags & FLAG_BLOCK_PATHFINDING)
+            if (TileIsUnderShape(i, j, shape, grid, m_ExpandPathfinding))
+                tile |= TILE_OBSTRUCTED_PATHFINDING;
+        if (shape.flags & FLAG_BLOCK_FOUNDATION)
+            if (TileIsUnderShape(i, j, shape, grid, m_ExpandFoundation))
+                tile |= TILE_OBSTRUCTED_FOUNDATION;
+
+        ++range.first;
+    }
+
+    grid.set(i, j, tile);
+}
+
+void CCmpObstructionManager::UpdateTilesUnderShape(StaticShape const & shape, Grid<u8>& grid, entity_pos_t const & expand)
+{
+    CFixedVector2D center(shape.x, shape.z);
+    CFixedVector2D halfSize(shape.hw + expand, shape.hh + expand);
+    CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(shape.u, shape.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 r = it->second.r + expandPathfinding;
+            entity_pos_t x, z;
+            TileCenter(i, j, x, z);
+            if (Geometry::PointIsInSquare(CFixedVector2D(x, z) - center, shape.u, shape.v, halfSize))
+                UpdateTile(i, j, grid);
+        }
+    }
+}
+void CCmpObstructionManager::UpdateTilesUnderShape(UnitShape const & shape, Grid<u8>& grid, entity_pos_t const & expand)
+{
+    CFixedVector2D center(shape.x, shape.z);
+    entity_pos_t r = shape.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);
+    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)
+            UpdateTile(i, j, grid);
+}
+
+bool CCmpObstructionManager::Rasterise(Grid<u8>& grid)
+{
+    if (!IsDirty(grid))
+        return false;
+
+    PROFILE("Rasterise");
+    TIMER_ACCRUE(tc_Rasterize);
+
+    if (!m_StaticHistory.isInHistory(grid.m_DirtyID) || !m_UnitHistory.isInHistory(grid.m_DirtyID))
+    {
+        // Some pieces of history are missing, we need to recompute the entire grid
+        grid.reset();
+        for (std::map<u32, StaticShape>::iterator it = m_StaticShapes.begin(); it != m_StaticShapes.end(); ++it)
+        {
+            if (it->second.flags & FLAG_BLOCK_PATHFINDING)
+                ModifyTilesUnderShape(it->second, grid, m_ExpandPathfinding, TILE_OBSTRUCTED_PATHFINDING);
+
+            if (it->second.flags & FLAG_BLOCK_FOUNDATION)
+                ModifyTilesUnderShape(it->second, grid, m_ExpandFoundation, TILE_OBSTRUCTED_FOUNDATION);
         }
 
-        if (it->second.flags & FLAG_BLOCK_FOUNDATION)
+        for (std::map<u32, UnitShape>::iterator it = m_UnitShapes.begin(); it != m_UnitShapes.end(); ++it)
         {
-            entity_pos_t r = it->second.r + expandFoundation;
+            if (it->second.flags & FLAG_BLOCK_PATHFINDING)
+                ModifyTilesUnderShape(it->second, grid, m_ExpandPathfinding, TILE_OBSTRUCTED_PATHFINDING);
 
-            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);
+            if (it->second.flags & FLAG_BLOCK_FOUNDATION)
+                ModifyTilesUnderShape(it->second, grid, m_ExpandFoundation, TILE_OBSTRUCTED_FOUNDATION);
         }
     }
+    else
+    {
+        // We'll recompute only needed tiles according to history
 
+        entity_pos_t maxExpand = std::max(m_ExpandPathfinding, m_ExpandFoundation);
+
+        // Compute tiles under modified static shapes
+        for (ShapeHistory<StaticShape>::const_iterator it = m_StaticHistory.get(grid.m_DirtyID); it != m_StaticHistory.end(); ++it)
+        {
+            if (it->second.flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION))
+                UpdateTilesUnderShape(it->second, grid, maxExpand);
+        }
+
+        // Reset tiles under removed unit shapes
+        for (ShapeHistory<UnitShape>::const_iterator it = m_UnitHistory.get(grid.m_DirtyID); it != m_UnitHistory.end(); ++it)
+        {
+            if (it->second.flags & (FLAG_BLOCK_PATHFINDING | FLAG_BLOCK_FOUNDATION))
+                UpdateTilesUnderShape(it->second, grid, maxExpand);
+        }
+    }
+
+    grid.m_DirtyID = m_DirtyID;
+
     // Any tiles outside or very near the edge of the map are impassable
 
     // WARNING: CCmpRangeManager::LosIsOffWorld needs to be kept in sync with this