Merge pull request #2171 from elsid/fix_recast_mesh_tile_cache

Fix update navmesh

apps/openmw_test_suite/CMakeLists.txt

@@ -24,6 +24,7 @@ if (GTEST_FOUND AND GMOCK_FOUND)
         detournavigator/gettilespositions.cpp
         detournavigator/recastmeshobject.cpp
         detournavigator/navmeshtilescache.cpp
+        detournavigator/tilecachedrecastmeshmanager.cpp
     )
 
     source_group(apps\\openmw_test_suite FILES openmw_test_suite.cpp ${UNITTEST_SRC_FILES})

apps/openmw_test_suite/detournavigator/tilecachedrecastmeshmanager.cpp

@@ -0,0 +1,123 @@
+#include "operators.hpp"
+
+#include <components/detournavigator/tilecachedrecastmeshmanager.hpp>
+#include <components/detournavigator/settingsutils.hpp>
+
+#include <BulletCollision/CollisionShapes/btBoxShape.h>
+#include <BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h>
+#include <BulletCollision/CollisionShapes/btTriangleMesh.h>
+#include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
+#include <BulletCollision/CollisionShapes/btCompoundShape.h>
+
+#include <gtest/gtest.h>
+
+namespace
+{
+    using namespace testing;
+    using namespace DetourNavigator;
+
+    struct DetourNavigatorTileCachedRecastMeshManagerTest : Test
+    {
+        Settings mSettings;
+
+        DetourNavigatorTileCachedRecastMeshManagerTest()
+        {
+            mSettings.mBorderSize = 16;
+            mSettings.mCellSize = 0.2f;
+            mSettings.mRecastScaleFactor = 0.017647058823529415f;
+            mSettings.mTileSize = 64;
+            mSettings.mTrianglesPerChunk = 256;
+        }
+    };
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, get_mesh_for_empty_should_return_nullptr)
+    {
+        TileCachedRecastMeshManager manager(mSettings);
+        EXPECT_EQ(manager.getMesh(TilePosition(0, 0)), nullptr);
+    }
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, has_tile_for_empty_should_return_false)
+    {
+        TileCachedRecastMeshManager manager(mSettings);
+        EXPECT_FALSE(manager.hasTile(TilePosition(0, 0)));
+    }
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, get_revision_for_empty_should_return_zero)
+    {
+        const TileCachedRecastMeshManager manager(mSettings);
+        EXPECT_EQ(manager.getRevision(), 0);
+    }
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, for_each_tile_position_for_empty_should_call_none)
+    {
+        TileCachedRecastMeshManager manager(mSettings);
+        std::size_t calls = 0;
+        manager.forEachTilePosition([&] (const TilePosition&) { ++calls; });
+        EXPECT_EQ(calls, 0);
+    }
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, get_mesh_after_add_object_should_return_recast_mesh_for_each_used_tile)
+    {
+        TileCachedRecastMeshManager manager(mSettings);
+        const btBoxShape boxShape(btVector3(20, 20, 100));
+        manager.addObject(ObjectId(1ul), boxShape, btTransform::getIdentity(), AreaType::AreaType_ground);
+        EXPECT_NE(manager.getMesh(TilePosition(-1, -1)), nullptr);
+        EXPECT_NE(manager.getMesh(TilePosition(-1, 0)), nullptr);
+        EXPECT_NE(manager.getMesh(TilePosition(0, -1)), nullptr);
+        EXPECT_NE(manager.getMesh(TilePosition(0, 0)), nullptr);
+    }
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, get_mesh_after_add_object_should_return_nullptr_for_unused_tile)
+    {
+        TileCachedRecastMeshManager manager(mSettings);
+        const btBoxShape boxShape(btVector3(20, 20, 100));
+        manager.addObject(ObjectId(1ul), boxShape, btTransform::getIdentity(), AreaType::AreaType_ground);
+        EXPECT_EQ(manager.getMesh(TilePosition(1, 0)), nullptr);
+    }
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, get_mesh_for_moved_object_should_return_recast_mesh_for_each_used_tile)
+    {
+        TileCachedRecastMeshManager manager(mSettings);
+        const btBoxShape boxShape(btVector3(20, 20, 100));
+        const btTransform transform(btMatrix3x3::getIdentity(), btVector3(getTileSize(mSettings) / mSettings.mRecastScaleFactor, 0, 0));
+
+        manager.addObject(ObjectId(1ul), boxShape, transform, AreaType::AreaType_ground);
+        EXPECT_NE(manager.getMesh(TilePosition(0, -1)), nullptr);
+        EXPECT_NE(manager.getMesh(TilePosition(0, 0)), nullptr);
+        EXPECT_NE(manager.getMesh(TilePosition(1, 0)), nullptr);
+        EXPECT_NE(manager.getMesh(TilePosition(1, -1)), nullptr);
+
+        manager.updateObject(ObjectId(1ul), boxShape, btTransform::getIdentity(), AreaType::AreaType_ground);
+        EXPECT_NE(manager.getMesh(TilePosition(-1, -1)), nullptr);
+        EXPECT_NE(manager.getMesh(TilePosition(-1, 0)), nullptr);
+        EXPECT_NE(manager.getMesh(TilePosition(0, -1)), nullptr);
+        EXPECT_NE(manager.getMesh(TilePosition(0, 0)), nullptr);
+    }
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, get_mesh_for_moved_object_should_return_nullptr_for_unused_tile)
+    {
+        TileCachedRecastMeshManager manager(mSettings);
+        const btBoxShape boxShape(btVector3(20, 20, 100));
+        const btTransform transform(btMatrix3x3::getIdentity(), btVector3(getTileSize(mSettings) / mSettings.mRecastScaleFactor, 0, 0));
+
+        manager.addObject(ObjectId(1ul), boxShape, transform, AreaType::AreaType_ground);
+        EXPECT_EQ(manager.getMesh(TilePosition(-1, -1)), nullptr);
+        EXPECT_EQ(manager.getMesh(TilePosition(-1, 0)), nullptr);
+
+        manager.updateObject(ObjectId(1ul), boxShape, btTransform::getIdentity(), AreaType::AreaType_ground);
+        EXPECT_EQ(manager.getMesh(TilePosition(1, 0)), nullptr);
+        EXPECT_EQ(manager.getMesh(TilePosition(1, -1)), nullptr);
+    }
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, get_mesh_for_removed_object_should_return_nullptr_for_all_previously_used_tiles)
+    {
+        TileCachedRecastMeshManager manager(mSettings);
+        const btBoxShape boxShape(btVector3(20, 20, 100));
+        manager.addObject(ObjectId(1ul), boxShape, btTransform::getIdentity(), AreaType::AreaType_ground);
+        manager.removeObject(ObjectId(1ul));
+        EXPECT_EQ(manager.getMesh(TilePosition(-1, -1)), nullptr);
+        EXPECT_EQ(manager.getMesh(TilePosition(-1, 0)), nullptr);
+        EXPECT_EQ(manager.getMesh(TilePosition(0, -1)), nullptr);
+        EXPECT_EQ(manager.getMesh(TilePosition(0, 0)), nullptr);
+    }
+}

components/detournavigator/navmeshmanager.cpp

@@ -44,9 +44,11 @@ namespace DetourNavigator
     bool NavMeshManager::updateObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform,
                                       const AreaType areaType)
     {
-        if (!mRecastMeshManager.updateObject(id, transform, areaType))
+        const auto changedTiles = mRecastMeshManager.updateObject(id, shape, transform, areaType);
+        if (changedTiles.empty())
             return false;
-        addChangedTiles(shape, transform, ChangeType::update);
+        for (const auto& tile : changedTiles)
+            addChangedTile(tile, ChangeType::update);
         return true;
     }
 

components/detournavigator/tilecachedrecastmeshmanager.cpp

@@ -15,48 +15,59 @@ namespace DetourNavigator
         bool result = false;
         auto& tilesPositions = mObjectsTilesPositions[id];
         const auto border = getBorderSize(mSettings);
-        getTilesPositions(shape, transform, mSettings, [&] (const TilePosition& tilePosition)
-            {
-                const auto tiles = mTiles.lock();
-                auto tile = tiles->find(tilePosition);
-                if (tile == tiles->end())
+        {
+            auto tiles = mTiles.lock();
+            getTilesPositions(shape, transform, mSettings, [&] (const TilePosition& tilePosition)
                 {
-                    auto tileBounds = makeTileBounds(mSettings, tilePosition);
-                    tileBounds.mMin -= osg::Vec2f(border, border);
-                    tileBounds.mMax += osg::Vec2f(border, border);
-                    tile = tiles->insert(std::make_pair(tilePosition,
-                            CachedRecastMeshManager(mSettings, tileBounds))).first;
-                }
-                if (tile->second.addObject(id, shape, transform, areaType))
-                {
-                    tilesPositions.push_back(tilePosition);
-                    result = true;
-                }
-            });
+                    if (addTile(id, shape, transform, areaType, tilePosition, border, tiles.get()))
+                    {
+                        tilesPositions.insert(tilePosition);
+                        result = true;
+                    }
+                });
+        }
         if (result)
             ++mRevision;
         return result;
     }
 
-    bool TileCachedRecastMeshManager::updateObject(const ObjectId id, const btTransform& transform,
-        const AreaType areaType)
+    std::vector<TilePosition> TileCachedRecastMeshManager::updateObject(const ObjectId id, const btCollisionShape& shape,
+        const btTransform& transform, const AreaType areaType)
     {
         const auto object = mObjectsTilesPositions.find(id);
         if (object == mObjectsTilesPositions.end())
-            return false;
-        bool result = false;
+            return std::vector<TilePosition>();
+        auto& currentTiles = object->second;
+        const auto border = getBorderSize(mSettings);
+        std::vector<TilePosition> changedTiles;
+        std::set<TilePosition> newTiles;
         {
-            const auto tiles = mTiles.lock();
-            for (const auto& tilePosition : object->second)
+            auto tiles = mTiles.lock();
+            const auto onTilePosition = [&] (const TilePosition& tilePosition)
             {
-                const auto tile = tiles->find(tilePosition);
-                if (tile != tiles->end())
-                    result = tile->second.updateObject(id, transform, areaType) || result;
-            }
+                if (currentTiles.count(tilePosition))
+                {
+                    if (updateTile(id, transform, areaType, tilePosition, tiles.get()))
+                    {
+                        newTiles.insert(tilePosition);
+                        changedTiles.push_back(tilePosition);
+                    }
+                }
+                else if (addTile(id, shape, transform, areaType, tilePosition, border, tiles.get()))
+                {
+                    newTiles.insert(tilePosition);
+                    changedTiles.push_back(tilePosition);
+                }
+            };
+            getTilesPositions(shape, transform, mSettings, onTilePosition);
+            for (const auto& tile : currentTiles)
+                if (!newTiles.count(tile) && removeTile(id, tile, tiles.get()))
+                    changedTiles.push_back(tile);
+            std::swap(currentTiles, newTiles);
         }
-        if (result)
+        if (!changedTiles.empty())
             ++mRevision;
-        return result;
+        return changedTiles;
     }
 
     boost::optional<RemovedRecastMeshObject> TileCachedRecastMeshManager::removeObject(const ObjectId id)
@@ -65,17 +76,14 @@ namespace DetourNavigator
         if (object == mObjectsTilesPositions.end())
             return boost::none;
         boost::optional<RemovedRecastMeshObject> result;
-        for (const auto& tilePosition : object->second)
         {
-            const auto tiles = mTiles.lock();
-            const auto tile = tiles->find(tilePosition);
-            if (tile == tiles->end())
-                continue;
-            const auto tileResult = tile->second.removeObject(id);
-            if (tile->second.isEmpty())
-                tiles->erase(tile);
-            if (tileResult && !result)
-                result = tileResult;
+            auto tiles = mTiles.lock();
+            for (const auto& tilePosition : object->second)
+            {
+                const auto removed = removeTile(id, tilePosition, tiles.get());
+                if (removed && !result)
+                    result = removed;
+            }
         }
         if (result)
             ++mRevision;
@@ -172,4 +180,38 @@ namespace DetourNavigator
     {
         return mRevision;
     }
+
+    bool TileCachedRecastMeshManager::addTile(const ObjectId id, const btCollisionShape& shape,
+        const btTransform& transform, const AreaType areaType, const TilePosition& tilePosition, float border,
+        std::map<TilePosition, CachedRecastMeshManager>& tiles)
+    {
+        auto tile = tiles.find(tilePosition);
+        if (tile == tiles.end())
+        {
+            auto tileBounds = makeTileBounds(mSettings, tilePosition);
+            tileBounds.mMin -= osg::Vec2f(border, border);
+            tileBounds.mMax += osg::Vec2f(border, border);
+            tile = tiles.insert(std::make_pair(tilePosition, CachedRecastMeshManager(mSettings, tileBounds))).first;
+        }
+        return tile->second.addObject(id, shape, transform, areaType);
+    }
+
+    bool TileCachedRecastMeshManager::updateTile(const ObjectId id, const btTransform& transform,
+        const AreaType areaType, const TilePosition& tilePosition, std::map<TilePosition, CachedRecastMeshManager>& tiles)
+    {
+        const auto tile = tiles.find(tilePosition);
+        return tile != tiles.end() && tile->second.updateObject(id, transform, areaType);
+    }
+
+    boost::optional<RemovedRecastMeshObject> TileCachedRecastMeshManager::removeTile(const ObjectId id,
+        const TilePosition& tilePosition, std::map<TilePosition, CachedRecastMeshManager>& tiles)
+    {
+        const auto tile = tiles.find(tilePosition);
+        if (tile == tiles.end())
+            return boost::optional<RemovedRecastMeshObject>();
+        const auto tileResult = tile->second.removeObject(id);
+        if (tile->second.isEmpty())
+            tiles.erase(tile);
+        return tileResult;
+    }
 }

components/detournavigator/tilecachedrecastmeshmanager.hpp

@@ -8,6 +8,7 @@
 
 #include <map>
 #include <mutex>
+#include <set>
 
 namespace DetourNavigator
 {
@@ -19,7 +20,8 @@ namespace DetourNavigator
         bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform,
                        const AreaType areaType);
 
-        bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType);
+        std::vector<TilePosition> updateObject(const ObjectId id, const btCollisionShape& shape,
+                                               const btTransform& transform, const AreaType areaType);
 
         boost::optional<RemovedRecastMeshObject> removeObject(const ObjectId id);
 
@@ -43,9 +45,19 @@ namespace DetourNavigator
     private:
         const Settings& mSettings;
         Misc::ScopeGuarded<std::map<TilePosition, CachedRecastMeshManager>> mTiles;
-        std::unordered_map<ObjectId, std::vector<TilePosition>> mObjectsTilesPositions;
+        std::unordered_map<ObjectId, std::set<TilePosition>> mObjectsTilesPositions;
         std::map<osg::Vec2i, std::vector<TilePosition>> mWaterTilesPositions;
         std::size_t mRevision = 0;
+
+        bool addTile(const ObjectId id, const btCollisionShape& shape, const btTransform& transform,
+                     const AreaType areaType, const TilePosition& tilePosition, float border,
+                     std::map<TilePosition, CachedRecastMeshManager>& tiles);
+
+        bool updateTile(const ObjectId id, const btTransform& transform, const AreaType areaType,
+                        const TilePosition& tilePosition, std::map<TilePosition, CachedRecastMeshManager>& tiles);
+
+        boost::optional<RemovedRecastMeshObject> removeTile(const ObjectId id, const TilePosition& tilePosition,
+                                                            std::map<TilePosition, CachedRecastMeshManager>& tiles);
     };
 }