Update NavMesh one by one tile in order from nearest to player

apps/openmw/mwworld/scene.cpp

@@ -363,7 +363,8 @@ namespace MWWorld
             else
                 mPhysics->disableWater();
 
-            navigator->update();
+            const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr();
+            navigator->update(player.getRefData().getPosition().asVec3());
 
             if (!cell->isExterior() && !(cell->getCell()->mData.mFlags & ESM::Cell::QuasiEx))
                 mRendering.configureAmbient(cell->getCell());

apps/openmw/mwworld/worldimp.cpp

@@ -19,6 +19,7 @@
 
 #include <components/sceneutil/positionattitudetransform.hpp>
 
+#include <components/detournavigator/debug.hpp>
 #include <components/detournavigator/navigator.hpp>
 #include <components/detournavigator/debug.hpp>
 

apps/openmw_test_suite/detournavigator/navigator.cpp

@@ -56,6 +56,32 @@ namespace
         }
     };
 
+    TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_throw_exception)
+    {
+        EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut), InvalidArgument);
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception)
+    {
+        mNavigator->addAgent(mAgentHalfExtents);
+        EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut), NavigatorException);
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, find_path_for_removed_agent_should_throw_exception)
+    {
+        mNavigator->addAgent(mAgentHalfExtents);
+        mNavigator->removeAgent(mAgentHalfExtents);
+        EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut), InvalidArgument);
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, add_agent_should_count_each_agent)
+    {
+        mNavigator->addAgent(mAgentHalfExtents);
+        mNavigator->addAgent(mAgentHalfExtents);
+        mNavigator->removeAgent(mAgentHalfExtents);
+        EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut), NavigatorException);
+    }
+
     TEST_F(DetourNavigatorNavigatorTest, update_then_find_path_should_return_path)
     {
         const std::array<btScalar, 5 * 5> heightfieldData {{
@@ -70,7 +96,7 @@ namespace
 
         mNavigator->addAgent(mAgentHalfExtents);
         mNavigator->addObject(1, shape, btTransform::getIdentity());
-        mNavigator->update();
+        mNavigator->update(mPlayerPosition);
         mNavigator->wait();
 
         mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut);
@@ -127,7 +153,7 @@ namespace
         mNavigator->addAgent(mAgentHalfExtents);
         mNavigator->addObject(1, shape, btTransform::getIdentity());
         mNavigator->addObject(2, shape2, btTransform::getIdentity());
-        mNavigator->update();
+        mNavigator->update(mPlayerPosition);
         mNavigator->wait();
 
         mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut);

components/detournavigator/asyncnavmeshupdater.cpp

@@ -7,6 +7,21 @@
 
 #include <iostream>
 
+namespace
+{
+    using DetourNavigator::TilePosition;
+
+    int getDistance(const TilePosition& lhs, const TilePosition& rhs)
+    {
+        return std::abs(lhs.x() - rhs.x()) + std::abs(lhs.y() - rhs.y());
+    }
+
+    std::pair<int, int> makePriority(const TilePosition& changedTile, const TilePosition& playerTile)
+    {
+        return std::make_pair(getDistance(changedTile, playerTile), getDistance(changedTile, TilePosition {0, 0}));
+    }
+}
+
 namespace DetourNavigator
 {
     AsyncNavMeshUpdater::AsyncNavMeshUpdater(const Settings& settings)
@@ -20,25 +35,21 @@ namespace DetourNavigator
     {
         mShouldStop = true;
         std::unique_lock<std::mutex> lock(mMutex);
-        mJobs.clear();
+        mJobs = decltype(mJobs)();
         mHasJob.notify_all();
         lock.unlock();
         mThread.join();
     }
 
     void AsyncNavMeshUpdater::post(const osg::Vec3f& agentHalfExtents, const std::shared_ptr<RecastMesh>& recastMesh,
-        const std::shared_ptr<NavMeshCacheItem>& navMeshCacheItem, std::set<TilePosition>&& changedTiles)
+        const std::shared_ptr<NavMeshCacheItem>& navMeshCacheItem, const TilePosition& playerTile,
+        const std::set<TilePosition>& changedTiles)
     {
         const std::lock_guard<std::mutex> lock(mMutex);
-        const auto job = mJobs.find(agentHalfExtents);
+        for (const auto& changedTile : changedTiles)
-        if (job == mJobs.end() || job->second.mChangedTiles.empty())
         {
-            mJobs[agentHalfExtents] = Job {agentHalfExtents, recastMesh, navMeshCacheItem, std::move(changedTiles)};
+            mJobs.push(Job {agentHalfExtents, recastMesh, navMeshCacheItem, changedTile,
-        }
+                            makePriority(changedTile, playerTile)});
-        else
-        {
-            job->second.mRecastMesh = recastMesh;
-            job->second.mChangedTiles.insert(changedTiles.begin(), changedTiles.end());
         }
         mHasJob.notify_all();
     }
@@ -74,11 +85,8 @@ namespace DetourNavigator
 
         const auto start = std::chrono::steady_clock::now();
 
-        if (job.mNavMeshCacheItem->mValue && !job.mChangedTiles.empty())
+        updateNavMesh(job.mAgentHalfExtents, *job.mRecastMesh, job.mChangedTile, mSettings,
-            updateNavMesh(job.mAgentHalfExtents, *job.mRecastMesh, job.mChangedTiles, mSettings,
+            job.mNavMeshCacheItem->mValue);
-                *job.mNavMeshCacheItem->mValue);
-        else
-            job.mNavMeshCacheItem->mValue = makeNavMesh(job.mAgentHalfExtents, *job.mRecastMesh, mSettings);
 
         const auto finish = std::chrono::steady_clock::now();
 
@@ -101,8 +109,8 @@ namespace DetourNavigator
             return boost::none;
         }
         log("got ", mJobs.size(), " jobs");
-        const auto job = mJobs.begin()->second;
+        const auto job = mJobs.top();
-        mJobs.erase(mJobs.begin());
+        mJobs.pop();
         return job;
     }
 
@@ -115,6 +123,6 @@ namespace DetourNavigator
         if (mSettings.get().mEnableWriteRecastMeshToFile)
             writeToFile(*job.mRecastMesh, mSettings.get().mRecastMeshPathPrefix, revision);
         if (mSettings.get().mEnableWriteNavMeshToFile)
-            writeToFile(*job.mNavMeshCacheItem->mValue, mSettings.get().mNavMeshPathPrefix, revision);
+            writeToFile(*job.mNavMeshCacheItem->mValue.lock(), mSettings.get().mNavMeshPathPrefix, revision);
     }
 }

components/detournavigator/asyncnavmeshupdater.hpp

@@ -2,6 +2,7 @@
 #define OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H
 
 #include "recastmesh.hpp"
+#include "sharednavmesh.hpp"
 #include "tileposition.hpp"
 
 #include <osg/Vec3f>
@@ -13,6 +14,7 @@
 #include <map>
 #include <memory>
 #include <mutex>
+#include <queue>
 #include <set>
 #include <thread>
 
@@ -20,11 +22,9 @@ class dtNavMesh;
 
 namespace DetourNavigator
 {
-    using NavMeshPtr = std::shared_ptr<dtNavMesh>;
-
     struct NavMeshCacheItem
     {
-        NavMeshPtr mValue;
+        SharedNavMesh mValue;
         std::size_t mRevision;
     };
 
@@ -35,7 +35,8 @@ namespace DetourNavigator
         ~AsyncNavMeshUpdater();
 
         void post(const osg::Vec3f& agentHalfExtents, const std::shared_ptr<RecastMesh>& recastMesh,
-            const std::shared_ptr<NavMeshCacheItem>& mNavMeshCacheItem, std::set<TilePosition>&& changedTiles);
+            const std::shared_ptr<NavMeshCacheItem>& navMeshCacheItem, const TilePosition& playerTile,
+            const std::set<TilePosition>& changedTiles);
 
         void wait();
 
@@ -45,10 +46,16 @@ namespace DetourNavigator
             osg::Vec3f mAgentHalfExtents;
             std::shared_ptr<RecastMesh> mRecastMesh;
             std::shared_ptr<NavMeshCacheItem> mNavMeshCacheItem;
-            std::set<TilePosition> mChangedTiles;
+            TilePosition mChangedTile;
+            std::pair<int, int> mPriority;
+
+            friend inline bool operator <(const Job& lhs, const Job& rhs)
+            {
+                return lhs.mPriority > rhs.mPriority;
+            }
         };
 
-        using Jobs = std::map<osg::Vec3f, Job>;
+        using Jobs = std::priority_queue<Job, std::deque<Job>>;
 
         std::reference_wrapper<const Settings> mSettings;
         std::atomic_bool mShouldStop;

components/detournavigator/exceptions.hpp

@@ -10,6 +10,12 @@ namespace DetourNavigator
         NavigatorException(const std::string& message) : std::runtime_error(message) {}
         NavigatorException(const char* message) : std::runtime_error(message) {}
     };
+
+    struct InvalidArgument : std::invalid_argument
+    {
+        InvalidArgument(const std::string& message) : std::invalid_argument(message) {}
+        InvalidArgument(const char* message) : std::invalid_argument(message) {}
+    };
 }
 
 #endif

components/detournavigator/makenavmesh.cpp

@@ -6,7 +6,7 @@
 #include "recastmesh.hpp"
 #include "settings.hpp"
 #include "settingsutils.hpp"
-#include "debug.hpp"
+#include "sharednavmesh.hpp"
 
 #include <DetourNavMesh.h>
 #include <DetourNavMeshBuilder.h>
@@ -218,22 +218,8 @@ namespace
 
 namespace DetourNavigator
 {
-    NavMeshPtr makeEmptyNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
+    NavMeshPtr makeEmptyNavMesh(const Settings& settings)
-            const Settings& settings)
     {
-        log("build empty NavMesh:",
-            " 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()),
-            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;
@@ -242,7 +228,7 @@ namespace DetourNavigator
         const auto maxPolysPerTile = 1 << polyBits;
 
         dtNavMeshParams params;
-        rcVcopy(params.orig, boundsMin.ptr());
+        std::fill_n(params.orig, 3, 0.0f);
         params.tileWidth = settings.mTileSize * settings.mCellSize;
         params.tileHeight = settings.mTileSize * settings.mCellSize;
         params.maxTiles = maxTiles;
@@ -254,67 +240,8 @@ namespace DetourNavigator
         return navMesh;
     }
 
-    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()),
-            boundsMin.ptr(), boundsMax.ptr());
-
-        const auto minTilePosition = getTilePosition(settings, boundsMin);
-        const auto maxTilePosition = getTilePosition(settings, boundsMax);
-
-        // 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;
-        std::fill_n(params.orig, 3, 0.0f);
-        params.tileWidth = getTileSize(settings);
-        params.tileHeight = getTileSize(settings);
-        params.maxTiles = maxTiles;
-        params.maxPolys = maxPolysPerTile;
-
-        NavMeshPtr navMesh(dtAllocNavMesh(), &dtFreeNavMesh);
-        OPENMW_CHECK_DT_STATUS(navMesh->init(&params));
-
-        for (int y = minTilePosition.y(); y <= maxTilePosition.y(); ++y)
-        {
-            for (int x = minTilePosition.x(); x <= maxTilePosition.x(); ++x)
-            {
-                const auto tileBounds = makeTileBounds(settings, TilePosition(x, y));
-                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();
-            }
-        }
-
-        return navMesh;
-    }
-
     void updateNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
-            const std::set<TilePosition>& changedTiles, const Settings& settings, dtNavMesh& navMesh)
+        const TilePosition& changedTile, const Settings& settings, SharedNavMesh& navMesh)
     {
         log("update NavMesh with mutiple tiles:",
             " agentHeight=", std::setprecision(std::numeric_limits<float>::max_exponent10),
@@ -323,24 +250,25 @@ namespace DetourNavigator
             getMaxClimb(settings),
             " agentRadius=", std::setprecision(std::numeric_limits<float>::max_exponent10),
             getRadius(settings, agentHalfExtents),
-            " changedTiles.size()=", changedTiles.size());
+            " changedTile=", changedTile);
 
         osg::Vec3f boundsMin;
         osg::Vec3f boundsMax;
         rcCalcBounds(recastMesh.getVertices().data(), int(recastMesh.getVerticesCount()),
             boundsMin.ptr(), boundsMax.ptr());
 
-        const auto& params = *navMesh.getParams();
+        const auto& params = *navMesh.lock()->getParams();
         const osg::Vec3f origin(params.orig[0], params.orig[1], params.orig[2]);
 
-        for (const auto& tilePosition : changedTiles)
+        const auto x = changedTile.x();
+        const auto y = changedTile.y();
+
         {
-            const auto x = tilePosition.x();
+            const auto locked = navMesh.lock();
-            const auto y = tilePosition.y();
+            locked->removeTile(locked->getTileRefAt(x, y, 0), nullptr, nullptr);
+        }
 
-            navMesh.removeTile(navMesh.getTileRefAt(x, y, 0), nullptr, nullptr);
+        const auto tileBounds = makeTileBounds(settings, changedTile);
-
-            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());
 
@@ -348,11 +276,10 @@ namespace DetourNavigator
             tileBorderMin, tileBorderMax, settings);
 
         if (!navMeshData.mValue)
-                continue;
+            return;
 
-            OPENMW_CHECK_DT_STATUS(navMesh.addTile(navMeshData.mValue.get(), navMeshData.mSize,
+        OPENMW_CHECK_DT_STATUS(navMesh.lock()->addTile(navMeshData.mValue.get(), navMeshData.mSize,
                                                         DT_TILE_FREE_DATA, 0, 0));
         navMeshData.mValue.release();
     }
-    }
 }

components/detournavigator/makenavmesh.hpp

@@ -13,18 +13,15 @@ class dtNavMesh;
 namespace DetourNavigator
 {
     class RecastMesh;
+    class SharedNavMesh;
     struct Settings;
 
     using NavMeshPtr = std::shared_ptr<dtNavMesh>;
 
-    NavMeshPtr makeEmptyNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
+    NavMeshPtr makeEmptyNavMesh(const Settings& settings);
-            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);
+            const TilePosition& changedTile, const Settings& settings, SharedNavMesh& navMesh);
 }
 
 #endif

components/detournavigator/navigator.cpp

@@ -13,6 +13,7 @@ namespace DetourNavigator
     void Navigator::addAgent(const osg::Vec3f& agentHalfExtents)
     {
         ++mAgents[agentHalfExtents];
+        mNavMeshManager.addAgent(agentHalfExtents);
     }
 
     void Navigator::removeAgent(const osg::Vec3f& agentHalfExtents)
@@ -29,10 +30,10 @@ namespace DetourNavigator
         return mNavMeshManager.removeObject(id);
     }
 
-    void Navigator::update()
+    void Navigator::update(const osg::Vec3f& playerPosition)
     {
         for (const auto& v : mAgents)
-            mNavMeshManager.update(v.first);
+            mNavMeshManager.update(playerPosition, v.first);
     }
 
     void Navigator::wait()

components/detournavigator/navigator.hpp

@@ -25,7 +25,7 @@ namespace DetourNavigator
 
         bool removeObject(std::size_t id);
 
-        void update();
+        void update(const osg::Vec3f& playerPosition);
 
         void wait();
 
@@ -34,9 +34,7 @@ namespace DetourNavigator
             const osg::Vec3f& end, OutputIterator out) const
         {
             const auto navMesh = mNavMeshManager.getNavMesh(agentHalfExtents);
-            if (!navMesh)
+            return findSmoothPath(*navMesh.lock(), toNavMeshCoordinates(mSettings, agentHalfExtents),
-                return out;
-            return findSmoothPath(*navMesh, toNavMeshCoordinates(mSettings, agentHalfExtents),
                 toNavMeshCoordinates(mSettings, start), toNavMeshCoordinates(mSettings, end), mSettings, out);
         }
 

components/detournavigator/navmeshmanager.cpp

@@ -1,7 +1,9 @@
 #include "navmeshmanager.hpp"
 #include "debug.hpp"
+#include "exceptions.hpp"
 #include "makenavmesh.hpp"
 #include "settings.hpp"
+#include "sharednavmesh.hpp"
 
 #include <DetourNavMesh.h>
 
@@ -27,34 +29,39 @@ namespace DetourNavigator
         return true;
     }
 
+    void NavMeshManager::addAgent(const osg::Vec3f& agentHalfExtents)
+    {
+        auto cached = mCache.find(agentHalfExtents);
+        if (cached != mCache.end())
+            return;
+        mCache.insert(std::make_pair(agentHalfExtents,
+            std::make_shared<NavMeshCacheItem>(NavMeshCacheItem {makeEmptyNavMesh(mSettings), mRevision}))
+        );
+        log("cache add for agent=", agentHalfExtents);
+    }
+
     void NavMeshManager::reset(const osg::Vec3f& agentHalfExtents)
     {
         mCache.erase(agentHalfExtents);
     }
 
-    void NavMeshManager::update(const osg::Vec3f& agentHalfExtents)
+    void NavMeshManager::update(osg::Vec3f playerPosition, const osg::Vec3f& agentHalfExtents)
     {
-        auto cached = mCache.find(agentHalfExtents);
+        const auto& cached = getCached(agentHalfExtents);
-        if (cached == mCache.end())
+        if (cached->mRevision >= mRevision)
-            cached = mCache.insert(std::make_pair(agentHalfExtents,
-                std::make_shared<NavMeshCacheItem>(NavMeshCacheItem {
-                    makeEmptyNavMesh(agentHalfExtents, *mRecastMeshManager.getMesh(), mSettings),
-                    mRevision
-                }))).first;
-        else if (cached->second->mRevision >= mRevision)
             return;
-        cached->second->mRevision = mRevision;
+        cached->mRevision = mRevision;
         const auto changedTiles = mChangedTiles.find(agentHalfExtents);
-        if (changedTiles == mChangedTiles.end())
+        if (changedTiles != mChangedTiles.end())
         {
-            mAsyncNavMeshUpdater.post(agentHalfExtents, mRecastMeshManager.getMesh(), cached->second,
+            TilePosition playerTile;
-                                      std::set<TilePosition>());
+            playerPosition *= mSettings.mRecastScaleFactor;
-        }
+            std::swap(playerPosition.y(), playerPosition.z());
-        else
+            cached->mValue.raw()->calcTileLoc(playerPosition.ptr(), &playerTile.x(), &playerTile.y());
-        {
+            mAsyncNavMeshUpdater.post(agentHalfExtents, mRecastMeshManager.getMesh(), cached, playerTile,
-            mAsyncNavMeshUpdater.post(agentHalfExtents, mRecastMeshManager.getMesh(), cached->second,
+                                      changedTiles->second);
-                                      std::move(changedTiles->second));
             mChangedTiles.erase(changedTiles);
+            log("cache update posted for agent=", agentHalfExtents);
         }
     }
 
@@ -63,12 +70,9 @@ namespace DetourNavigator
         mAsyncNavMeshUpdater.wait();
     }
 
-    NavMeshConstPtr NavMeshManager::getNavMesh(const osg::Vec3f& agentHalfExtents) const
+    SharedNavMesh NavMeshManager::getNavMesh(const osg::Vec3f& agentHalfExtents) const
     {
-        const auto it = mCache.find(agentHalfExtents);
+        return getCached(agentHalfExtents)->mValue;
-        if (it == mCache.end())
-            return nullptr;
-        return it->second->mValue;
     }
 
     void NavMeshManager::addChangedTiles(const btCollisionShape& shape, const btTransform& transform)
@@ -84,18 +88,16 @@ namespace DetourNavigator
         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);
+                item->mValue.raw()->calcTileLoc(min.ptr(), &minTileX, &minTileY);
 
                 int maxTileX;
                 int maxTileY;
-                    navMesh->calcTileLoc(max.ptr(), &maxTileX, &maxTileY);
+                item->mValue.raw()->calcTileLoc(max.ptr(), &maxTileX, &maxTileY);
 
                 if (minTileX > maxTileX)
                     std::swap(minTileX, maxTileX);
@@ -109,5 +111,14 @@ namespace DetourNavigator
             }
         }
     }
+
+    const std::shared_ptr<NavMeshCacheItem>& NavMeshManager::getCached(const osg::Vec3f& agentHalfExtents) const
+    {
+        const auto cached = mCache.find(agentHalfExtents);
+        if (cached != mCache.end())
+            return cached->second;
+        std::ostringstream stream;
+        stream << "Agent with half extents is not found: " << agentHalfExtents;
+        throw InvalidArgument(stream.str());
     }
 }

components/detournavigator/navmeshmanager.hpp

@@ -3,6 +3,9 @@
 
 #include "asyncnavmeshupdater.hpp"
 #include "cachedrecastmeshmanager.hpp"
+#include "sharednavmesh.hpp"
+
+#include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
 
 #include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
 
@@ -15,8 +18,6 @@ class dtNavMesh;
 
 namespace DetourNavigator
 {
-    using NavMeshConstPtr = std::shared_ptr<const dtNavMesh>;
-
     class NavMeshManager
     {
     public:
@@ -34,13 +35,15 @@ namespace DetourNavigator
 
         bool removeObject(std::size_t id);
 
+        void addAgent(const osg::Vec3f& agentHalfExtents);
+
         void reset(const osg::Vec3f& agentHalfExtents);
 
-        void update(const osg::Vec3f& agentHalfExtents);
+        void update(osg::Vec3f playerPosition, const osg::Vec3f& agentHalfExtents);
 
         void wait();
 
-        NavMeshConstPtr getNavMesh(const osg::Vec3f& agentHalfExtents) const;
+        SharedNavMesh getNavMesh(const osg::Vec3f& agentHalfExtents) const;
 
     private:
         std::size_t mRevision = 0;
@@ -51,6 +54,8 @@ namespace DetourNavigator
         AsyncNavMeshUpdater mAsyncNavMeshUpdater;
 
         void addChangedTiles(const btCollisionShape& shape, const btTransform& transform);
+
+        const std::shared_ptr<NavMeshCacheItem>& getCached(const osg::Vec3f& agentHalfExtents) const;
     };
 }
 

components/detournavigator/sharednavmesh.hpp

@@ -0,0 +1,58 @@
+#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SHAREDNAVMESH_H
+#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SHAREDNAVMESH_H
+
+#include <mutex>
+#include <memory>
+
+class dtNavMesh;
+
+namespace DetourNavigator
+{
+    using NavMeshPtr = std::shared_ptr<dtNavMesh>;
+
+    class LockedSharedNavMesh
+    {
+    public:
+        LockedSharedNavMesh(std::mutex& mutex, const NavMeshPtr& value)
+            : mLock(new std::lock_guard<std::mutex>(mutex)), mValue(value)
+        {}
+
+        dtNavMesh* operator ->() const
+        {
+            return mValue.get();
+        }
+
+        dtNavMesh& operator *() const
+        {
+            return *mValue;
+        }
+
+    private:
+        std::unique_ptr<const std::lock_guard<std::mutex>> mLock;
+        NavMeshPtr mValue;
+    };
+
+    class SharedNavMesh
+    {
+    public:
+        SharedNavMesh(const NavMeshPtr& value)
+            : mMutex(std::make_shared<std::mutex>()), mValue(value)
+        {}
+
+        LockedSharedNavMesh lock() const
+        {
+            return LockedSharedNavMesh(*mMutex, mValue);
+        }
+
+        NavMeshPtr raw() const
+        {
+            return mValue;
+        }
+
+    private:
+        std::shared_ptr<std::mutex> mMutex;
+        NavMeshPtr mValue;
+    };
+}
+
+#endif