Build NavMesh tile data only for changed tiles

components/detournavigator/asyncnavmeshupdater.cpp

@@ -10,8 +10,7 @@
 namespace DetourNavigator
 {
     AsyncNavMeshUpdater::AsyncNavMeshUpdater(const Settings& settings)
-        : mSettings(settings)
+        : mSettings(std::cref(settings))
-        , mMaxRevision(0)
         , mShouldStop()
         , mThread([&] { process(); })
     {
@@ -28,10 +27,19 @@ namespace DetourNavigator
     }
 
     void AsyncNavMeshUpdater::post(const osg::Vec3f& agentHalfExtents, const std::shared_ptr<RecastMesh>& recastMesh,
-        const std::shared_ptr<NavMeshCacheItem>& navMeshCacheItem)
+        const std::shared_ptr<NavMeshCacheItem>& navMeshCacheItem, std::set<TilePosition>&& changedTiles)
     {
         const std::lock_guard<std::mutex> lock(mMutex);
-        mJobs[agentHalfExtents] = Job {agentHalfExtents, recastMesh, navMeshCacheItem};
+        const auto job = mJobs.find(agentHalfExtents);
+        if (job == mJobs.end() || job->second.mChangedTiles.empty())
+        {
+            mJobs[agentHalfExtents] = Job {agentHalfExtents, recastMesh, navMeshCacheItem, std::move(changedTiles)};
+        }
+        else
+        {
+            job->second.mRecastMesh = recastMesh;
+            job->second.mChangedTiles.insert(changedTiles.begin(), changedTiles.end());
+        }
         mHasJob.notify_all();
     }
 
@@ -62,18 +70,15 @@ namespace DetourNavigator
 
     void AsyncNavMeshUpdater::processJob(const Job& job)
     {
-        log("process job for agent=", job.mAgentHalfExtents,
+        log("process job for agent=", job.mAgentHalfExtents);
-            " revision=", job.mNavMeshCacheItem->mRevision,
-            " max_revision=", mMaxRevision);
-
-        if (job.mNavMeshCacheItem->mRevision < mMaxRevision)
-            return;
-
-        mMaxRevision = job.mNavMeshCacheItem->mRevision;
 
         const auto start = std::chrono::steady_clock::now();
 
-        job.mNavMeshCacheItem->mValue = makeNavMesh(job.mAgentHalfExtents, *job.mRecastMesh, mSettings);
+        if (job.mNavMeshCacheItem->mValue && !job.mChangedTiles.empty())
+            updateNavMesh(job.mAgentHalfExtents, *job.mRecastMesh, job.mChangedTiles, mSettings,
+                *job.mNavMeshCacheItem->mValue);
+        else
+            job.mNavMeshCacheItem->mValue = makeNavMesh(job.mAgentHalfExtents, *job.mRecastMesh, mSettings);
 
         const auto finish = std::chrono::steady_clock::now();
 

components/detournavigator/asyncnavmeshupdater.hpp

@@ -2,6 +2,7 @@
 #define OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H
 
 #include "recastmesh.hpp"
+#include "tileposition.hpp"
 
 #include <osg/Vec3f>
 
@@ -12,22 +13,19 @@
 #include <map>
 #include <memory>
 #include <mutex>
+#include <set>
 #include <thread>
 
 class dtNavMesh;
 
 namespace DetourNavigator
 {
-    using NavMeshConstPtr = std::shared_ptr<const dtNavMesh>;
+    using NavMeshPtr = std::shared_ptr<dtNavMesh>;
 
     struct NavMeshCacheItem
     {
-        NavMeshConstPtr mValue = nullptr;
+        NavMeshPtr mValue;
         std::size_t mRevision;
-
-        NavMeshCacheItem(std::size_t mRevision)
-            : mRevision(mRevision)
-        {}
     };
 
     class AsyncNavMeshUpdater
@@ -37,7 +35,7 @@ namespace DetourNavigator
         ~AsyncNavMeshUpdater();
 
         void post(const osg::Vec3f& agentHalfExtents, const std::shared_ptr<RecastMesh>& recastMesh,
-                  const std::shared_ptr<NavMeshCacheItem>& navMeshCacheItem);
+            const std::shared_ptr<NavMeshCacheItem>& mNavMeshCacheItem, std::set<TilePosition>&& changedTiles);
 
         void wait();
 
@@ -47,12 +45,12 @@ namespace DetourNavigator
             osg::Vec3f mAgentHalfExtents;
             std::shared_ptr<RecastMesh> mRecastMesh;
             std::shared_ptr<NavMeshCacheItem> mNavMeshCacheItem;
+            std::set<TilePosition> mChangedTiles;
         };
 
         using Jobs = std::map<osg::Vec3f, Job>;
 
         std::reference_wrapper<const Settings> mSettings;
-        std::atomic_size_t mMaxRevision;
         std::atomic_bool mShouldStop;
         std::mutex mMutex;
         std::condition_variable mHasJob;

components/detournavigator/cachedrecastmeshmanager.cpp

@@ -8,12 +8,12 @@ namespace DetourNavigator
     {
     }
 
-    bool CachedRecastMeshManager::removeObject(std::size_t id)
+    boost::optional<RecastMeshManager::Object> CachedRecastMeshManager::removeObject(std::size_t id)
     {
-        if (!mImpl.removeObject(id))
+        const auto object = mImpl.removeObject(id);
-            return false;
+        if (object)
-        mCached.reset();
+            mCached.reset();
-        return true;
+        return object;
     }
 
     std::shared_ptr<RecastMesh> CachedRecastMeshManager::getMesh()
@@ -21,5 +21,5 @@ namespace DetourNavigator
         if (!mCached)
             mCached = mImpl.getMesh();
         return mCached;
-    }
+}
 }

components/detournavigator/cachedrecastmeshmanager.hpp

@@ -21,7 +21,7 @@ namespace DetourNavigator
             return true;
         }
 
-        bool removeObject(std::size_t id);
+        boost::optional<RecastMeshManager::Object> removeObject(std::size_t id);
 
         std::shared_ptr<RecastMesh> getMesh();
 

components/detournavigator/makenavmesh.cpp

@@ -5,13 +5,16 @@
 #include "recastmesh.hpp"
 #include "settings.hpp"
 #include "settingsutils.hpp"
+#include "debug.hpp"
 
 #include <DetourNavMesh.h>
 #include <DetourNavMeshBuilder.h>
 #include <Recast.h>
 #include <RecastAlloc.h>
 
-#include <boost/optional.hpp>
+#include <iostream>
+#include <iomanip>
+#include <limits>
 
 namespace
 {
@@ -211,22 +214,57 @@ namespace
 
         return NavMeshData(navMeshData, navMeshDataSize);
     }
+}
 
-    int nextPow2(int v)
+namespace DetourNavigator
+{
+    NavMeshPtr makeEmptyNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
+            const Settings& settings)
     {
-        v--;
+        log("build empty NavMesh:",
-        v |= v >> 1;
+            " agentHeight=", std::setprecision(std::numeric_limits<float>::max_exponent10),
-        v |= v >> 2;
+            getHeight(settings, agentHalfExtents),
-        v |= v >> 4;
+            " agentMaxClimb=", std::setprecision(std::numeric_limits<float>::max_exponent10),
-        v |= v >> 8;
+            getMaxClimb(settings),
-        v |= v >> 16;
+            " agentRadius=", std::setprecision(std::numeric_limits<float>::max_exponent10),
-        v++;
+            getRadius(settings, agentHalfExtents));
-        return v;
+
+        osg::Vec3f boundsMin;
+        osg::Vec3f boundsMax;
+        rcCalcBounds(recastMesh.getVertices().data(), static_cast<int>(recastMesh.getVerticesCount()),
+            boundsMin.ptr(), boundsMax.ptr());
+
+        // Max tiles and max polys affect how the tile IDs are caculated.
+        // There are 22 bits available for identifying a tile and a polygon.
+        const auto tileBits = 10;
+        const auto polyBits = 22 - tileBits;
+        const auto maxTiles = 1 << tileBits;
+        const auto maxPolysPerTile = 1 << polyBits;
+
+        dtNavMeshParams params;
+        rcVcopy(params.orig, boundsMin.ptr());
+        params.tileWidth = settings.mTileSize * settings.mCellSize;
+        params.tileHeight = settings.mTileSize * settings.mCellSize;
+        params.maxTiles = maxTiles;
+        params.maxPolys = maxPolysPerTile;
+
+        NavMeshPtr navMesh(dtAllocNavMesh(), &dtFreeNavMesh);
+        OPENMW_CHECK_DT_STATUS(navMesh->init(&params));
+
+        return navMesh;
     }
 
-    NavMeshPtr makeNavMeshWithMultiTiles(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
+    NavMeshPtr makeNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
         const Settings& settings)
     {
+        log("build NavMesh with mutiple tiles:",
+            " agentHeight=", std::setprecision(std::numeric_limits<float>::max_exponent10),
+            getHeight(settings, agentHalfExtents),
+            " agentMaxClimb=", std::setprecision(std::numeric_limits<float>::max_exponent10),
+            getMaxClimb(settings),
+            " agentRadius=", std::setprecision(std::numeric_limits<float>::max_exponent10),
+            getRadius(settings, agentHalfExtents));
+
         osg::Vec3f boundsMin;
         osg::Vec3f boundsMax;
         rcCalcBounds(recastMesh.getVertices().data(), static_cast<int>(recastMesh.getVerticesCount()),
@@ -235,12 +273,9 @@ namespace
         const auto minTilePosition = getTilePosition(settings, boundsMin);
         const auto maxTilePosition = getTilePosition(settings, boundsMax);
 
-        const auto tileWidth = maxTilePosition.x() - minTilePosition.x() + 1;
-        const auto tileHeight = maxTilePosition.y() - minTilePosition.y() + 1;
-
         // Max tiles and max polys affect how the tile IDs are caculated.
         // There are 22 bits available for identifying a tile and a polygon.
-        const auto tileBits = std::min(static_cast<int>(std::log2(nextPow2(tileWidth * tileHeight))), 14);
+        const auto tileBits = 10;
         const auto polyBits = 22 - tileBits;
         const auto maxTiles = 1 << tileBits;
         const auto maxPolysPerTile = 1 << polyBits;
@@ -277,12 +312,47 @@ namespace
 
         return navMesh;
     }
-}
 
-namespace DetourNavigator
+    void updateNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
-{
+            const std::set<TilePosition>& changedTiles, const Settings& settings, dtNavMesh& navMesh)
-    NavMeshPtr makeNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, const Settings& settings)
     {
-        return makeNavMeshWithMultiTiles(agentHalfExtents, recastMesh, settings);
+        log("update NavMesh with mutiple tiles:",
+            " agentHeight=", std::setprecision(std::numeric_limits<float>::max_exponent10),
+            getHeight(settings, agentHalfExtents),
+            " agentMaxClimb=", std::setprecision(std::numeric_limits<float>::max_exponent10),
+            getMaxClimb(settings),
+            " agentRadius=", std::setprecision(std::numeric_limits<float>::max_exponent10),
+            getRadius(settings, agentHalfExtents),
+            " changedTiles.size()=", changedTiles.size());
+
+        osg::Vec3f boundsMin;
+        osg::Vec3f boundsMax;
+        rcCalcBounds(recastMesh.getVertices().data(), int(recastMesh.getVerticesCount()),
+            boundsMin.ptr(), boundsMax.ptr());
+
+        const auto& params = *navMesh.getParams();
+        const osg::Vec3f origin(params.orig[0], params.orig[1], params.orig[2]);
+
+        for (const auto& tilePosition : changedTiles)
+        {
+            const auto x = tilePosition.x();
+            const auto y = tilePosition.y();
+
+            navMesh.removeTile(navMesh.getTileRefAt(x, y, 0), nullptr, nullptr);
+
+            const auto tileBounds = makeTileBounds(settings, tilePosition);
+            const osg::Vec3f tileBorderMin(tileBounds.mMin.x(), boundsMin.y() - 1, tileBounds.mMin.y());
+            const osg::Vec3f tileBorderMax(tileBounds.mMax.x(), boundsMax.y() + 1, tileBounds.mMax.y());
+
+            auto navMeshData = makeNavMeshTileData(agentHalfExtents, recastMesh, x, y,
+                tileBorderMin, tileBorderMax, settings);
+
+            if (!navMeshData.mValue)
+                continue;
+
+            OPENMW_CHECK_DT_STATUS(navMesh.addTile(navMeshData.mValue.get(), navMeshData.mSize,
+                                                    DT_TILE_FREE_DATA, 0, 0));
+            navMeshData.mValue.release();
+        }
     }
 }

components/detournavigator/makenavmesh.hpp

@@ -1,9 +1,12 @@
 #ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H
 #define OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H
 
+#include "tileposition.hpp"
+
 #include <osg/Vec3f>
 
 #include <memory>
+#include <set>
 
 class dtNavMesh;
 
@@ -14,7 +17,14 @@ namespace DetourNavigator
 
     using NavMeshPtr = std::shared_ptr<dtNavMesh>;
 
-    NavMeshPtr makeNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, const Settings& settings);
+    NavMeshPtr makeEmptyNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
+            const Settings& settings);
+
+    NavMeshPtr makeNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
+            const Settings& settings);
+
+    void updateNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
+            const std::set<TilePosition>& changedTiles, const Settings& settings, dtNavMesh& navMesh);
 }
 
 #endif

components/detournavigator/navmeshmanager.cpp

@@ -1,21 +1,29 @@
 #include "navmeshmanager.hpp"
 #include "debug.hpp"
+#include "makenavmesh.hpp"
+#include "settings.hpp"
+
+#include <DetourNavMesh.h>
+
+#include <BulletCollision/CollisionShapes/btConcaveShape.h>
 
 #include <iostream>
 
 namespace DetourNavigator
 {
     NavMeshManager::NavMeshManager(const Settings& settings)
-        : mRecastMeshManager(settings)
+        : mSettings(settings)
+        , mRecastMeshManager(settings)
         , mAsyncNavMeshUpdater(settings)
-    {
+    {}
-    }
 
     bool NavMeshManager::removeObject(std::size_t id)
     {
-        if (!mRecastMeshManager.removeObject(id))
+        const auto object = mRecastMeshManager.removeObject(id);
+        if (!object)
             return false;
         ++mRevision;
+        addChangedTiles(*object->mShape, object->mTransform);
         return true;
     }
 
@@ -26,13 +34,28 @@ namespace DetourNavigator
 
     void NavMeshManager::update(const osg::Vec3f& agentHalfExtents)
     {
-        auto it = mCache.find(agentHalfExtents);
+        auto cached = mCache.find(agentHalfExtents);
-        if (it == mCache.end())
+        if (cached == mCache.end())
-            it = mCache.insert(std::make_pair(agentHalfExtents, std::make_shared<NavMeshCacheItem>(mRevision))).first;
+            cached = mCache.insert(std::make_pair(agentHalfExtents,
-        else if (it->second->mRevision >= mRevision)
+                std::make_shared<NavMeshCacheItem>(NavMeshCacheItem {
+                    makeEmptyNavMesh(agentHalfExtents, *mRecastMeshManager.getMesh(), mSettings),
+                    mRevision
+                }))).first;
+        else if (cached->second->mRevision >= mRevision)
             return;
-        it->second->mRevision = mRevision;
+        cached->second->mRevision = mRevision;
-        mAsyncNavMeshUpdater.post(agentHalfExtents, mRecastMeshManager.getMesh(), it->second);
+        const auto changedTiles = mChangedTiles.find(agentHalfExtents);
+        if (changedTiles == mChangedTiles.end())
+        {
+            mAsyncNavMeshUpdater.post(agentHalfExtents, mRecastMeshManager.getMesh(), cached->second,
+                                      std::set<TilePosition>());
+        }
+        else
+        {
+            mAsyncNavMeshUpdater.post(agentHalfExtents, mRecastMeshManager.getMesh(), cached->second,
+                                      std::move(changedTiles->second));
+            mChangedTiles.erase(changedTiles);
+        }
     }
 
     void NavMeshManager::wait()
@@ -47,4 +70,44 @@ namespace DetourNavigator
             return nullptr;
         return it->second->mValue;
     }
+
+    void NavMeshManager::addChangedTiles(const btCollisionShape& shape, const btTransform& transform)
+    {
+        btVector3 aabbMin;
+        btVector3 aabbMax;
+        shape.getAabb(transform, aabbMin, aabbMax);
+        osg::Vec3f min(aabbMin.x(), aabbMin.z(), aabbMin.y());
+        osg::Vec3f max(aabbMax.x(), aabbMax.z(), aabbMax.y());
+        min *= mSettings.mRecastScaleFactor;
+        max *= mSettings.mRecastScaleFactor;
+
+        for (auto& v : mCache)
+        {
+            if (const auto& item = v.second)
+            {
+                if (const auto& navMesh = item->mValue)
+                {
+                    auto& changedTiles = mChangedTiles[v.first];
+
+                    int minTileX;
+                    int minTileY;
+                    navMesh->calcTileLoc(min.ptr(), &minTileX, &minTileY);
+
+                    int maxTileX;
+                    int maxTileY;
+                    navMesh->calcTileLoc(max.ptr(), &maxTileX, &maxTileY);
+
+                    if (minTileX > maxTileX)
+                        std::swap(minTileX, maxTileX);
+
+                    if (minTileY > maxTileY)
+                        std::swap(minTileY, maxTileY);
+
+                    for (int tileX = minTileX; tileX <= maxTileX; ++tileX)
+                        for (int tileY = minTileY; tileY <= maxTileY; ++tileY)
+                            changedTiles.insert(TilePosition {tileX, tileY});
+                }
+            }
+        }
+    }
 }

components/detournavigator/navmeshmanager.hpp

@@ -4,6 +4,8 @@
 #include "asyncnavmeshupdater.hpp"
 #include "cachedrecastmeshmanager.hpp"
 
+#include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
+
 #include <osg/Vec3f>
 
 #include <map>
@@ -13,6 +15,8 @@ class dtNavMesh;
 
 namespace DetourNavigator
 {
+    using NavMeshConstPtr = std::shared_ptr<const dtNavMesh>;
+
     class NavMeshManager
     {
     public:
@@ -24,6 +28,7 @@ namespace DetourNavigator
             if (!mRecastMeshManager.addObject(id, shape, transform))
                 return false;
             ++mRevision;
+            addChangedTiles(shape, transform);
             return true;
         }
 
@@ -39,9 +44,13 @@ namespace DetourNavigator
 
     private:
         std::size_t mRevision = 0;
+        const Settings& mSettings;
         CachedRecastMeshManager mRecastMeshManager;
         std::map<osg::Vec3f, std::shared_ptr<NavMeshCacheItem>> mCache;
+        std::map<osg::Vec3f, std::set<TilePosition>> mChangedTiles;
         AsyncNavMeshUpdater mAsyncNavMeshUpdater;
+
+        void addChangedTiles(const btCollisionShape& shape, const btTransform& transform);
     };
 }
 

components/detournavigator/recastmeshmanager.cpp

@@ -7,8 +7,7 @@ namespace DetourNavigator
     RecastMeshManager::RecastMeshManager(const Settings& settings)
         : mShouldRebuild(false)
         , mMeshBuilder(settings)
-    {
+    {}
-    }
 
     bool RecastMeshManager::addObject(std::size_t id, const btHeightfieldTerrainShape& shape, const btTransform& transform)
     {
@@ -26,12 +25,15 @@ namespace DetourNavigator
         return true;
     }
 
-    bool RecastMeshManager::removeObject(std::size_t id)
+    boost::optional<RecastMeshManager::Object> RecastMeshManager::removeObject(std::size_t id)
     {
-        if (!mObjects.erase(id))
+        const auto object = mObjects.find(id);
-            return false;
+        if (object == mObjects.end())
+            return boost::none;
+        const auto result = object->second;
+        mObjects.erase(object);
         mShouldRebuild = true;
-        return true;
+        return result;
     }
 
     std::shared_ptr<RecastMesh> RecastMeshManager::getMesh()

components/detournavigator/recastmeshmanager.hpp

@@ -5,6 +5,8 @@
 
 #include <LinearMath/btTransform.h>
 
+#include <boost/optional.hpp>
+
 #include <unordered_map>
 
 class btCollisionShape;
@@ -14,23 +16,23 @@ namespace DetourNavigator
     class RecastMeshManager
     {
     public:
-        RecastMeshManager(const Settings& settings);
-
-        bool addObject(std::size_t id, const btHeightfieldTerrainShape& shape, const btTransform& transform);
-
-        bool addObject(std::size_t id, const btConcaveShape& shape, const btTransform& transform);
-
-        bool removeObject(std::size_t id);
-
-        std::shared_ptr<RecastMesh> getMesh();
-
-    private:
         struct Object
         {
             const btCollisionShape* mShape;
             btTransform mTransform;
         };
 
+        RecastMeshManager(const Settings& settings);
+
+        bool addObject(std::size_t id, const btHeightfieldTerrainShape& shape, const btTransform& transform);
+
+        bool addObject(std::size_t id, const btConcaveShape& shape, const btTransform& transform);
+
+        boost::optional<Object> removeObject(std::size_t id);
+
+        std::shared_ptr<RecastMesh> getMesh();
+
+    private:
         bool mShouldRebuild;
         RecastMeshBuilder mMeshBuilder;
         std::unordered_map<std::size_t, Object> mObjects;