Merge pull request #2318 from elsid/navmesh_lock_tile

Avoid work duplication for multiple thread async navmesh updater

components/detournavigator/asyncnavmeshupdater.cpp

@@ -126,8 +126,12 @@ namespace DetourNavigator
             try
             {
                 if (auto job = getNextJob())
-                    if (!processJob(*job))
+                {
+                    const auto processed = processJob(*job);
+                    unlockTile(job->mAgentHalfExtents, job->mChangedTile);
+                    if (!processed)
                         repost(std::move(*job));
+                }
             }
             catch (const std::exception& e)
             {
@@ -178,19 +182,47 @@ namespace DetourNavigator
     boost::optional<AsyncNavMeshUpdater::Job> AsyncNavMeshUpdater::getNextJob()
     {
         std::unique_lock<std::mutex> lock(mMutex);
-        if (!mHasJob.wait_for(lock, std::chrono::milliseconds(10), [&] { return !mJobs.empty(); }))
+
+        const auto threadId = std::this_thread::get_id();
+        auto& threadQueue = mThreadsQueues[threadId];
+
+        while (true)
         {
-            mFirstStart.lock()->reset();
-            mDone.notify_all();
-            return boost::none;
+            const auto hasJob = [&] { return !mJobs.empty() || !threadQueue.mPushed.empty(); };
+
+            if (!mHasJob.wait_for(lock, std::chrono::milliseconds(10), hasJob))
+            {
+                mFirstStart.lock()->reset();
+                mDone.notify_all();
+                return boost::none;
+            }
+
+            Log(Debug::Debug) << "Got " << mJobs.size() << " navigator jobs and "
+                << threadQueue.mJobs.size() << " thread jobs";
+
+            auto job = threadQueue.mJobs.empty()
+                ? getJob(mJobs, mPushed)
+                : getJob(threadQueue.mJobs, threadQueue.mPushed);
+
+            const auto owner = lockTile(job.mAgentHalfExtents, job.mChangedTile);
+
+            if (owner == threadId)
+                return job;
+
+            postThreadJob(std::move(job), mThreadsQueues[owner]);
         }
-        Log(Debug::Debug) << "Got " << mJobs.size() << " navigator jobs";
-        const auto job = mJobs.top();
-        mJobs.pop();
-        const auto pushed = mPushed.find(job.mAgentHalfExtents);
-        pushed->second.erase(job.mChangedTile);
-        if (pushed->second.empty())
-            mPushed.erase(pushed);
+    }
+
+    AsyncNavMeshUpdater::Job AsyncNavMeshUpdater::getJob(Jobs& jobs, Pushed& pushed)
+    {
+        auto job = jobs.top();
+        jobs.pop();
+
+        const auto it = pushed.find(job.mAgentHalfExtents);
+        it->second.erase(job.mChangedTile);
+        if (it->second.empty())
+            pushed.erase(it);
+
         return job;
     }
 
@@ -239,4 +271,60 @@ namespace DetourNavigator
             mHasJob.notify_all();
         }
     }
+
+    void AsyncNavMeshUpdater::postThreadJob(Job&& job, Queue& queue)
+    {
+        if (queue.mPushed[job.mAgentHalfExtents].insert(job.mChangedTile).second)
+        {
+            queue.mJobs.push(std::move(job));
+            mHasJob.notify_all();
+        }
+    }
+
+    std::thread::id AsyncNavMeshUpdater::lockTile(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile)
+    {
+        if (mSettings.get().mAsyncNavMeshUpdaterThreads <= 1)
+            return std::this_thread::get_id();
+
+        auto locked = mProcessingTiles.lock();
+
+        auto agent = locked->find(agentHalfExtents);
+        if (agent == locked->end())
+        {
+            const auto threadId = std::this_thread::get_id();
+            locked->emplace(agentHalfExtents, std::map<TilePosition, std::thread::id>({{changedTile, threadId}}));
+            return threadId;
+        }
+
+        auto tile = agent->second.find(changedTile);
+        if (tile == agent->second.end())
+        {
+            const auto threadId = std::this_thread::get_id();
+            agent->second.emplace(changedTile, threadId);
+            return threadId;
+        }
+
+        return tile->second;
+    }
+
+    void AsyncNavMeshUpdater::unlockTile(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile)
+    {
+        if (mSettings.get().mAsyncNavMeshUpdaterThreads <= 1)
+            return;
+
+        auto locked = mProcessingTiles.lock();
+
+        auto agent = locked->find(agentHalfExtents);
+        if (agent == locked->end())
+            return;
+
+        auto tile = agent->second.find(changedTile);
+        if (tile == agent->second.end())
+            return;
+
+        agent->second.erase(tile);
+
+        if (agent->second.empty())
+            locked->erase(agent);
+    }
 }

components/detournavigator/asyncnavmeshupdater.hpp

@@ -69,6 +69,15 @@ namespace DetourNavigator
         };
 
         using Jobs = std::priority_queue<Job, std::deque<Job>>;
+        using Pushed = std::map<osg::Vec3f, std::set<TilePosition>>;
+
+        struct Queue
+        {
+            Jobs mJobs;
+            Pushed mPushed;
+
+            Queue() = default;
+        };
 
         std::reference_wrapper<const Settings> mSettings;
         std::reference_wrapper<TileCachedRecastMeshManager> mRecastMeshManager;
@@ -82,6 +91,8 @@ namespace DetourNavigator
         Misc::ScopeGuarded<TilePosition> mPlayerTile;
         Misc::ScopeGuarded<boost::optional<std::chrono::steady_clock::time_point>> mFirstStart;
         NavMeshTilesCache mNavMeshTilesCache;
+        Misc::ScopeGuarded<std::map<osg::Vec3f, std::map<TilePosition, std::thread::id>>> mProcessingTiles;
+        std::map<std::thread::id, Queue> mThreadsQueues;
         std::vector<std::thread> mThreads;
 
         void process() throw();
@@ -90,11 +101,19 @@ namespace DetourNavigator
 
         boost::optional<Job> getNextJob();
 
+        static Job getJob(Jobs& jobs, Pushed& pushed);
+
+        void postThreadJob(Job&& job, Queue& queue);
+
         void writeDebugFiles(const Job& job, const RecastMesh* recastMesh) const;
 
         std::chrono::steady_clock::time_point setFirstStart(const std::chrono::steady_clock::time_point& value);
 
         void repost(Job&& job);
+
+        std::thread::id lockTile(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile);
+
+        void unlockTile(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile);
     };
 }