Fix update navmesh

Every updated object should produce a set of changed tiles where it is placed. Before this change only current object tiles were updated. If object was moved to another set of tiles then navmesh were not changed in new tiles. TileCachedRecastMeshManager::updateObject should add all new tiles if object was moved and remove all no more used tiles. Both new and old tiles should be marked as changed. Also add tests to show desired result for add, update, remove.
6 years ago · da6df818ff
parent 4a9abf1c1b
commit da6df818ff
5 changed files with 223 additions and 43 deletions
--- a/apps/openmw_test_suite/CMakeLists.txt
+++ b/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})
--- a/apps/openmw_test_suite/detournavigator/tilecachedrecastmeshmanager.cpp
+++ b/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);
+    }
+}
--- a/components/detournavigator/navmeshmanager.cpp
+++ b/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;
    }

--- a/components/detournavigator/tilecachedrecastmeshmanager.cpp
+++ b/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 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))
+        {
+            auto tiles = mTiles.lock();
+            getTilesPositions(shape, transform, mSettings, [&] (const TilePosition& tilePosition)
                {
-                    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;
+    }
 }
--- a/components/detournavigator/tilecachedrecastmeshmanager.hpp
+++ b/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);
    };
 }