From d0ea9c482a9a489ae9b0b4dd93a0c0d1d73db4f6 Mon Sep 17 00:00:00 2001
From: elsid <elsid.mail@gmail.com>
Date: Fri, 14 May 2021 19:57:11 +0200
Subject: [PATCH] Reorder async navmesh updater jobs when player tile changes

When player tile changes distance to player that is part of jobs priority is
invalidated. So jobs are no longer in the right order. This can lead to
processing of farests tiles first.

Sort queue each time player tile is changed.
---
 .../detournavigator/asyncnavmeshupdater.cpp   | 38 ++++++++++++++-----
 .../detournavigator/asyncnavmeshupdater.hpp   |  7 ++--
 2 files changed, 33 insertions(+), 12 deletions(-)

diff --git a/components/detournavigator/asyncnavmeshupdater.cpp b/components/detournavigator/asyncnavmeshupdater.cpp
index ff51e39aad..3f7114b85c 100644
--- a/components/detournavigator/asyncnavmeshupdater.cpp
+++ b/components/detournavigator/asyncnavmeshupdater.cpp
@@ -88,13 +88,22 @@ namespace DetourNavigator
         const SharedNavMeshCacheItem& navMeshCacheItem, const TilePosition& playerTile,
         const std::map<TilePosition, ChangeType>& changedTiles)
     {
-        *mPlayerTile.lock() = playerTile;
+        bool playerTileChanged = false;
+        {
+            auto locked = mPlayerTile.lock();
+            playerTileChanged = *locked != playerTile;
+            *locked = playerTile;
+        }
 
-        if (changedTiles.empty())
+        if (!playerTileChanged && changedTiles.empty())
             return;
 
         const std::lock_guard<std::mutex> lock(mMutex);
 
+        if (playerTileChanged)
+            for (auto& job : mJobs)
+                job.mDistanceToPlayer = getManhattanDistance(job.mChangedTile, playerTile);
+
         for (const auto& changedTile : changedTiles)
         {
             if (mPushed[agentHalfExtents].insert(changedTile.first).second)
@@ -112,10 +121,21 @@ namespace DetourNavigator
                     ? mLastUpdates[job.mAgentHalfExtents][job.mChangedTile] + mSettings.get().mMinUpdateInterval
                     : std::chrono::steady_clock::time_point();
 
-                mJobs.push(std::move(job));
+                if (playerTileChanged)
+                {
+                    mJobs.push_back(std::move(job));
+                }
+                else
+                {
+                    const auto it = std::upper_bound(mJobs.begin(), mJobs.end(), job);
+                    mJobs.insert(it, std::move(job));
+                }
             }
         }
 
+        if (playerTileChanged)
+            std::sort(mJobs.begin(), mJobs.end());
+
         Log(Debug::Debug) << "Posted " << mJobs.size() << " navigator jobs";
 
         if (!mJobs.empty())
@@ -283,7 +303,7 @@ namespace DetourNavigator
         while (true)
         {
             const auto hasJob = [&] {
-                return (!mJobs.empty() && mJobs.top().mProcessTime <= std::chrono::steady_clock::now())
+                return (!mJobs.empty() && mJobs.front().mProcessTime <= std::chrono::steady_clock::now())
                     || !threadQueue.mJobs.empty();
             };
 
@@ -318,11 +338,11 @@ namespace DetourNavigator
     {
         const auto now = std::chrono::steady_clock::now();
 
-        if (jobs.top().mProcessTime > now)
+        if (jobs.front().mProcessTime > now)
             return {};
 
-        Job job = jobs.top();
-        jobs.pop();
+        Job job = jobs.front();
+        jobs.pop_front();
 
         if (changeLastUpdate && job.mChangeType == ChangeType::update)
             mLastUpdates[job.mAgentHalfExtents][job.mChangedTile] = now;
@@ -376,7 +396,7 @@ namespace DetourNavigator
         if (mPushed[job.mAgentHalfExtents].insert(job.mChangedTile).second)
         {
             ++job.mTryNumber;
-            mJobs.push(std::move(job));
+            mJobs.push_back(std::move(job));
             mHasJob.notify_all();
         }
     }
@@ -385,7 +405,7 @@ namespace DetourNavigator
     {
         if (queue.mPushed[job.mAgentHalfExtents].insert(job.mChangedTile).second)
         {
-            queue.mJobs.push(std::move(job));
+            queue.mJobs.push_back(std::move(job));
             mHasJob.notify_all();
         }
     }
diff --git a/components/detournavigator/asyncnavmeshupdater.hpp b/components/detournavigator/asyncnavmeshupdater.hpp
index c28d8f21db..fb95325191 100644
--- a/components/detournavigator/asyncnavmeshupdater.hpp
+++ b/components/detournavigator/asyncnavmeshupdater.hpp
@@ -14,9 +14,10 @@
 #include <condition_variable>
 #include <memory>
 #include <mutex>
-#include <queue>
+#include <deque>
 #include <set>
 #include <thread>
+#include <tuple>
 
 class dtNavMesh;
 
@@ -83,11 +84,11 @@ namespace DetourNavigator
 
             friend inline bool operator <(const Job& lhs, const Job& rhs)
             {
-                return lhs.getPriority() > rhs.getPriority();
+                return lhs.getPriority() < rhs.getPriority();
             }
         };
 
-        using Jobs = std::priority_queue<Job, std::deque<Job>>;
+        using Jobs = std::deque<Job>;
         using Pushed = std::map<osg::Vec3f, std::set<TilePosition>>;
 
         struct Queue