Use R-tree for dynamic priority of navmesh async job

apps/openmw_test_suite/detournavigator/asyncnavmeshupdater.cpp

@@ -267,7 +267,6 @@ namespace
         updater.wait(WaitConditionType::allJobsDone, &mListener);
         updater.stop();
         const std::set<TilePosition> present{
-            TilePosition(-2, 0),
             TilePosition(-1, -1),
             TilePosition(-1, 0),
             TilePosition(-1, 1),
@@ -278,6 +277,7 @@ namespace
             TilePosition(0, 2),
             TilePosition(1, -1),
             TilePosition(1, 0),
+            TilePosition(1, 1),
         };
         for (int x = -5; x <= 5; ++x)
             for (int y = -5; y <= 5; ++y)
@@ -336,4 +336,273 @@ namespace
         EXPECT_EQ(tile->mTileId, 2);
         EXPECT_EQ(tile->mVersion, navMeshFormatVersion);
     }
+
+    TEST_F(DetourNavigatorAsyncNavMeshUpdaterTest, repeated_tile_updates_should_be_delayed)
+    {
+        mRecastMeshManager.setWorldspace(mWorldspace, nullptr);
+
+        mSettings.mMaxTilesNumber = 9;
+        mSettings.mMinUpdateInterval = std::chrono::milliseconds(250);
+
+        AsyncNavMeshUpdater updater(mSettings, mRecastMeshManager, mOffMeshConnectionsManager, nullptr);
+        const auto navMeshCacheItem = std::make_shared<GuardedNavMeshCacheItem>(1, mSettings);
+
+        std::map<TilePosition, ChangeType> changedTiles;
+
+        for (int x = -3; x <= 3; ++x)
+            for (int y = -3; y <= 3; ++y)
+                changedTiles.emplace(TilePosition{ x, y }, ChangeType::update);
+
+        updater.post(mAgentBounds, navMeshCacheItem, mPlayerTile, mWorldspace, changedTiles);
+
+        updater.wait(WaitConditionType::allJobsDone, &mListener);
+
+        {
+            const AsyncNavMeshUpdaterStats stats = updater.getStats();
+            EXPECT_EQ(stats.mJobs, 0);
+            EXPECT_EQ(stats.mWaiting.mDelayed, 0);
+        }
+
+        updater.post(mAgentBounds, navMeshCacheItem, mPlayerTile, mWorldspace, changedTiles);
+
+        {
+            const AsyncNavMeshUpdaterStats stats = updater.getStats();
+            EXPECT_EQ(stats.mJobs, 49);
+            EXPECT_EQ(stats.mWaiting.mDelayed, 49);
+        }
+
+        updater.wait(WaitConditionType::allJobsDone, &mListener);
+
+        {
+            const AsyncNavMeshUpdaterStats stats = updater.getStats();
+            EXPECT_EQ(stats.mJobs, 0);
+            EXPECT_EQ(stats.mWaiting.mDelayed, 0);
+        }
+    }
+
+    struct DetourNavigatorSpatialJobQueueTest : Test
+    {
+        const AgentBounds mAgentBounds{ CollisionShapeType::Aabb, osg::Vec3f(1, 1, 1) };
+        const std::shared_ptr<GuardedNavMeshCacheItem> mNavMeshCacheItemPtr;
+        const std::weak_ptr<GuardedNavMeshCacheItem> mNavMeshCacheItem = mNavMeshCacheItemPtr;
+        const std::string_view mWorldspace = "worldspace";
+        const TilePosition mChangedTile{ 0, 0 };
+        const std::chrono::steady_clock::time_point mProcessTime{};
+        const TilePosition mPlayerTile{ 0, 0 };
+        const int mMaxTiles = 9;
+    };
+
+    TEST_F(DetourNavigatorSpatialJobQueueTest, should_store_multiple_jobs_per_tile)
+    {
+        std::list<Job> jobs;
+        SpatialJobQueue queue;
+
+        queue.push(jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, "worldspace1", mChangedTile,
+            ChangeType::remove, mProcessTime));
+        queue.push(jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, "worldspace2", mChangedTile,
+            ChangeType::update, mProcessTime));
+
+        ASSERT_EQ(queue.size(), 2);
+
+        const auto job1 = queue.pop(mChangedTile);
+        ASSERT_TRUE(job1.has_value());
+        EXPECT_EQ((*job1)->mWorldspace, "worldspace1");
+
+        const auto job2 = queue.pop(mChangedTile);
+        ASSERT_TRUE(job2.has_value());
+        EXPECT_EQ((*job2)->mWorldspace, "worldspace2");
+
+        EXPECT_EQ(queue.size(), 0);
+    }
+
+    struct DetourNavigatorJobQueueTest : DetourNavigatorSpatialJobQueueTest
+    {
+    };
+
+    TEST_F(DetourNavigatorJobQueueTest, pop_should_return_nullptr_from_empty)
+    {
+        JobQueue queue;
+        ASSERT_FALSE(queue.hasJob());
+        ASSERT_FALSE(queue.pop(mPlayerTile).has_value());
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, push_on_change_type_remove_should_add_to_removing)
+    {
+        const std::chrono::steady_clock::time_point processTime{};
+
+        std::list<Job> jobs;
+        const JobIt job = jobs.emplace(
+            jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, mChangedTile, ChangeType::remove, processTime);
+
+        JobQueue queue;
+        queue.push(job);
+
+        EXPECT_EQ(queue.getStats().mRemoving, 1);
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, pop_should_return_last_removing)
+    {
+        std::list<Job> jobs;
+        JobQueue queue;
+
+        queue.push(jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, TilePosition(0, 0),
+            ChangeType::remove, mProcessTime));
+        queue.push(jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, TilePosition(1, 0),
+            ChangeType::remove, mProcessTime));
+
+        ASSERT_TRUE(queue.hasJob());
+        const auto job = queue.pop(mPlayerTile);
+        ASSERT_TRUE(job.has_value());
+        EXPECT_EQ((*job)->mChangedTile, TilePosition(1, 0));
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, push_on_change_type_not_remove_should_add_to_updating)
+    {
+        std::list<Job> jobs;
+        const JobIt job = jobs.emplace(
+            jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, mChangedTile, ChangeType::update, mProcessTime);
+
+        JobQueue queue;
+        queue.push(job);
+
+        EXPECT_EQ(queue.getStats().mUpdating, 1);
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, pop_should_return_nearest_to_player_tile)
+    {
+        std::list<Job> jobs;
+
+        JobQueue queue;
+        queue.push(jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, TilePosition(0, 0),
+            ChangeType::update, mProcessTime));
+        queue.push(jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, TilePosition(1, 0),
+            ChangeType::update, mProcessTime));
+
+        ASSERT_TRUE(queue.hasJob());
+        const auto job = queue.pop(TilePosition(1, 0));
+        ASSERT_TRUE(job.has_value());
+        EXPECT_EQ((*job)->mChangedTile, TilePosition(1, 0));
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, push_on_processing_time_more_than_now_should_add_to_delayed)
+    {
+        const std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
+        const std::chrono::steady_clock::time_point processTime = now + std::chrono::seconds(1);
+
+        std::list<Job> jobs;
+        const JobIt job = jobs.emplace(
+            jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, mChangedTile, ChangeType::update, processTime);
+
+        JobQueue queue;
+        queue.push(job, now);
+
+        EXPECT_EQ(queue.getStats().mDelayed, 1);
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, pop_should_return_when_delayed_job_is_ready)
+    {
+        const std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
+        const std::chrono::steady_clock::time_point processTime = now + std::chrono::seconds(1);
+
+        std::list<Job> jobs;
+        const JobIt job = jobs.emplace(
+            jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, mChangedTile, ChangeType::update, processTime);
+
+        JobQueue queue;
+        queue.push(job, now);
+
+        ASSERT_FALSE(queue.hasJob(now));
+        ASSERT_FALSE(queue.pop(mPlayerTile, now).has_value());
+
+        ASSERT_TRUE(queue.hasJob(processTime));
+        EXPECT_TRUE(queue.pop(mPlayerTile, processTime).has_value());
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, update_should_move_ready_delayed_to_updating)
+    {
+        const std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
+        const std::chrono::steady_clock::time_point processTime = now + std::chrono::seconds(1);
+
+        std::list<Job> jobs;
+        const JobIt job = jobs.emplace(
+            jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, mChangedTile, ChangeType::update, processTime);
+
+        JobQueue queue;
+        queue.push(job, now);
+
+        ASSERT_EQ(queue.getStats().mDelayed, 1);
+
+        queue.update(mPlayerTile, mMaxTiles, processTime);
+
+        EXPECT_EQ(queue.getStats().mDelayed, 0);
+        EXPECT_EQ(queue.getStats().mUpdating, 1);
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, update_should_move_ready_delayed_to_removing_when_out_of_range)
+    {
+        const std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
+        const std::chrono::steady_clock::time_point processTime = now + std::chrono::seconds(1);
+
+        std::list<Job> jobs;
+        const JobIt job = jobs.emplace(
+            jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, mChangedTile, ChangeType::update, processTime);
+
+        JobQueue queue;
+        queue.push(job, now);
+
+        ASSERT_EQ(queue.getStats().mDelayed, 1);
+
+        queue.update(TilePosition(10, 10), mMaxTiles, processTime);
+
+        EXPECT_EQ(queue.getStats().mDelayed, 0);
+        EXPECT_EQ(queue.getStats().mRemoving, 1);
+        EXPECT_EQ(job->mChangeType, ChangeType::remove);
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, update_should_move_updating_to_removing_when_out_of_range)
+    {
+        std::list<Job> jobs;
+
+        JobQueue queue;
+        queue.push(jobs.emplace(
+            jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, mChangedTile, ChangeType::update, mProcessTime));
+        queue.push(jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, TilePosition(10, 10),
+            ChangeType::update, mProcessTime));
+
+        ASSERT_EQ(queue.getStats().mUpdating, 2);
+
+        queue.update(TilePosition(10, 10), mMaxTiles);
+
+        EXPECT_EQ(queue.getStats().mUpdating, 1);
+        EXPECT_EQ(queue.getStats().mRemoving, 1);
+    }
+
+    TEST_F(DetourNavigatorJobQueueTest, clear_should_remove_all)
+    {
+        const std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
+        const std::chrono::steady_clock::time_point processTime = now + std::chrono::seconds(1);
+
+        std::list<Job> jobs;
+        const JobIt removing = jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace,
+            TilePosition(0, 0), ChangeType::remove, mProcessTime);
+        const JobIt updating = jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace,
+            TilePosition(1, 0), ChangeType::update, mProcessTime);
+        const JobIt delayed = jobs.emplace(jobs.end(), mAgentBounds, mNavMeshCacheItem, mWorldspace, TilePosition(2, 0),
+            ChangeType::update, processTime);
+
+        JobQueue queue;
+        queue.push(removing);
+        queue.push(updating);
+        queue.push(delayed, now);
+
+        ASSERT_EQ(queue.getStats().mRemoving, 1);
+        ASSERT_EQ(queue.getStats().mUpdating, 1);
+        ASSERT_EQ(queue.getStats().mDelayed, 1);
+
+        queue.clear();
+
+        EXPECT_EQ(queue.getStats().mRemoving, 0);
+        EXPECT_EQ(queue.getStats().mUpdating, 0);
+        EXPECT_EQ(queue.getStats().mDelayed, 0);
+    }
 }

components/detournavigator/asyncnavmeshupdater.cpp

@@ -16,8 +16,9 @@
 
 #include <osg/io_utils>
 
+#include <boost/geometry.hpp>
+
 #include <algorithm>
-#include <numeric>
 #include <optional>
 #include <set>
 #include <tuple>
@@ -49,40 +50,6 @@ namespace DetourNavigator
             return false;
         }
 
-        auto getPriority(const Job& job) noexcept
-        {
-            return std::make_tuple(-static_cast<std::underlying_type_t<JobState>>(job.mState), job.mProcessTime,
-                job.mChangeType, job.mTryNumber, job.mDistanceToPlayer, job.mDistanceToOrigin);
-        }
-
-        struct LessByJobPriority
-        {
-            bool operator()(JobIt lhs, JobIt rhs) const noexcept { return getPriority(*lhs) < getPriority(*rhs); }
-        };
-
-        void insertPrioritizedJob(JobIt job, std::deque<JobIt>& queue)
-        {
-            const auto it = std::upper_bound(queue.begin(), queue.end(), job, LessByJobPriority{});
-            queue.insert(it, job);
-        }
-
-        auto getDbPriority(const Job& job) noexcept
-        {
-            return std::make_tuple(static_cast<std::underlying_type_t<JobState>>(job.mState), job.mChangeType,
-                job.mDistanceToPlayer, job.mDistanceToOrigin);
-        }
-
-        struct LessByJobDbPriority
-        {
-            bool operator()(JobIt lhs, JobIt rhs) const noexcept { return getDbPriority(*lhs) < getDbPriority(*rhs); }
-        };
-
-        void insertPrioritizedDbJob(JobIt job, std::deque<JobIt>& queue)
-        {
-            const auto it = std::upper_bound(queue.begin(), queue.end(), job, LessByJobDbPriority{});
-            queue.insert(it, job);
-        }
-
         auto getAgentAndTile(const Job& job) noexcept
         {
             return std::make_tuple(job.mAgentBounds, job.mChangedTile);
@@ -97,16 +64,6 @@ namespace DetourNavigator
                 settings.mRecast, settings.mWriteToNavMeshDb);
         }
 
-        void updateJobs(std::deque<JobIt>& jobs, TilePosition playerTile, int maxTiles)
-        {
-            for (JobIt job : jobs)
-            {
-                job->mDistanceToPlayer = getManhattanDistance(job->mChangedTile, playerTile);
-                if (!shouldAddTile(job->mChangedTile, playerTile, maxTiles))
-                    job->mChangeType = ChangeType::remove;
-            }
-        }
-
         std::size_t getNextJobId()
         {
             static std::atomic_size_t nextJobId{ 1 };
@@ -134,7 +91,7 @@ namespace DetourNavigator
     }
 
     Job::Job(const AgentBounds& agentBounds, std::weak_ptr<GuardedNavMeshCacheItem> navMeshCacheItem,
-        std::string_view worldspace, const TilePosition& changedTile, ChangeType changeType, int distanceToPlayer,
+        std::string_view worldspace, const TilePosition& changedTile, ChangeType changeType,
         std::chrono::steady_clock::time_point processTime)
         : mId(getNextJobId())
         , mAgentBounds(agentBounds)
@@ -143,11 +100,148 @@ namespace DetourNavigator
         , mChangedTile(changedTile)
         , mProcessTime(processTime)
         , mChangeType(changeType)
-        , mDistanceToPlayer(distanceToPlayer)
-        , mDistanceToOrigin(getManhattanDistance(changedTile, TilePosition{ 0, 0 }))
     {
     }
 
+    void SpatialJobQueue::clear()
+    {
+        mValues.clear();
+        mIndex.clear();
+        mSize = 0;
+    }
+
+    void SpatialJobQueue::push(JobIt job)
+    {
+        auto it = mValues.find(job->mChangedTile);
+
+        if (it == mValues.end())
+        {
+            it = mValues.emplace_hint(it, job->mChangedTile, std::deque<JobIt>());
+            mIndex.insert(IndexValue(IndexPoint(job->mChangedTile.x(), job->mChangedTile.y()), it));
+        }
+
+        it->second.push_back(job);
+
+        ++mSize;
+    }
+
+    std::optional<JobIt> SpatialJobQueue::pop(TilePosition playerTile)
+    {
+        const IndexPoint point(playerTile.x(), playerTile.y());
+        const auto it = mIndex.qbegin(boost::geometry::index::nearest(point, 1));
+
+        if (it == mIndex.qend())
+            return std::nullopt;
+
+        const UpdatingMap::iterator mapIt = it->second;
+        std::deque<JobIt>& tileJobs = mapIt->second;
+        JobIt result = tileJobs.front();
+        tileJobs.pop_front();
+
+        --mSize;
+
+        if (tileJobs.empty())
+        {
+            mValues.erase(mapIt);
+            mIndex.remove(*it);
+        }
+
+        return result;
+    }
+
+    void SpatialJobQueue::update(TilePosition playerTile, int maxTiles, std::vector<JobIt>& removing)
+    {
+        for (auto it = mValues.begin(); it != mValues.end();)
+        {
+            if (shouldAddTile(it->first, playerTile, maxTiles))
+            {
+                ++it;
+                continue;
+            }
+
+            for (JobIt job : it->second)
+            {
+                job->mChangeType = ChangeType::remove;
+                removing.push_back(job);
+            }
+
+            mSize -= it->second.size();
+            mIndex.remove(IndexValue(IndexPoint(it->first.x(), it->first.y()), it));
+            it = mValues.erase(it);
+        }
+    }
+
+    bool JobQueue::hasJob(std::chrono::steady_clock::time_point now) const
+    {
+        return !mRemoving.empty() || mUpdating.size() > 0
+            || (!mDelayed.empty() && mDelayed.front()->mProcessTime <= now);
+    }
+
+    void JobQueue::clear()
+    {
+        mRemoving.clear();
+        mDelayed.clear();
+        mUpdating.clear();
+    }
+
+    void JobQueue::push(JobIt job, std::chrono::steady_clock::time_point now)
+    {
+        if (job->mProcessTime > now)
+        {
+            mDelayed.push_back(job);
+            return;
+        }
+
+        if (job->mChangeType == ChangeType::remove)
+        {
+            mRemoving.push_back(job);
+            return;
+        }
+
+        mUpdating.push(job);
+    }
+
+    std::optional<JobIt> JobQueue::pop(TilePosition playerTile, std::chrono::steady_clock::time_point now)
+    {
+        if (!mRemoving.empty())
+        {
+            const JobIt result = mRemoving.back();
+            mRemoving.pop_back();
+            return result;
+        }
+
+        if (const std::optional<JobIt> result = mUpdating.pop(playerTile))
+            return result;
+
+        if (mDelayed.empty() || mDelayed.front()->mProcessTime > now)
+            return std::nullopt;
+
+        const JobIt result = mDelayed.front();
+        mDelayed.pop_front();
+        return result;
+    }
+
+    void JobQueue::update(TilePosition playerTile, int maxTiles, std::chrono::steady_clock::time_point now)
+    {
+        mUpdating.update(playerTile, maxTiles, mRemoving);
+
+        while (!mDelayed.empty() && mDelayed.front()->mProcessTime <= now)
+        {
+            const JobIt job = mDelayed.front();
+            mDelayed.pop_front();
+
+            if (shouldAddTile(job->mChangedTile, playerTile, maxTiles))
+            {
+                mUpdating.push(job);
+            }
+            else
+            {
+                job->mChangeType = ChangeType::remove;
+                mRemoving.push_back(job);
+            }
+        }
+    }
+
     AsyncNavMeshUpdater::AsyncNavMeshUpdater(const Settings& settings, TileCachedRecastMeshManager& recastMeshManager,
         OffMeshConnectionsManager& offMeshConnectionsManager, std::unique_ptr<NavMeshDb>&& db)
         : mSettings(settings)
@@ -183,42 +277,44 @@ namespace DetourNavigator
         std::unique_lock lock(mMutex);
 
         if (playerTileChanged)
-            updateJobs(mWaiting, playerTile, mSettings.get().mMaxTilesNumber);
+        {
+            Log(Debug::Debug) << "Player tile has been changed to " << playerTile;
+            mWaiting.update(playerTile, mSettings.get().mMaxTilesNumber);
+        }
 
         for (const auto& [changedTile, changeType] : changedTiles)
         {
             if (mPushed.emplace(agentBounds, changedTile).second)
             {
-                const auto processTime = changeType == ChangeType::update
-                    ? mLastUpdates[std::tie(agentBounds, changedTile)] + mSettings.get().mMinUpdateInterval
-                    : std::chrono::steady_clock::time_point();
-
-                const JobIt it = mJobs.emplace(mJobs.end(), agentBounds, navMeshCacheItem, worldspace, changedTile,
-                    changeType, getManhattanDistance(changedTile, playerTile), processTime);
+                const auto processTime = [&, changedTile = changedTile, changeType = changeType] {
+                    if (changeType != ChangeType::update)
+                        return std::chrono::steady_clock::time_point();
+                    const auto lastUpdate = mLastUpdates.find(std::tie(agentBounds, changedTile));
+                    if (lastUpdate == mLastUpdates.end())
+                        return std::chrono::steady_clock::time_point();
+                    return lastUpdate->second + mSettings.get().mMinUpdateInterval;
+                }();
+
+                const JobIt it = mJobs.emplace(
+                    mJobs.end(), agentBounds, navMeshCacheItem, worldspace, changedTile, changeType, processTime);
 
                 Log(Debug::Debug) << "Post job " << it->mId << " for agent=(" << it->mAgentBounds << ")"
-                                  << " changedTile=(" << it->mChangedTile << ") "
+                                  << " changedTile=(" << it->mChangedTile << ")"
                                   << " changeType=" << it->mChangeType;
 
-                if (playerTileChanged)
-                    mWaiting.push_back(it);
-                else
-                    insertPrioritizedJob(it, mWaiting);
+                mWaiting.push(it);
             }
         }
 
-        if (playerTileChanged)
-            std::sort(mWaiting.begin(), mWaiting.end(), LessByJobPriority{});
-
         Log(Debug::Debug) << "Posted " << mJobs.size() << " navigator jobs";
 
-        if (!mWaiting.empty())
+        if (mWaiting.hasJob())
             mHasJob.notify_all();
 
         lock.unlock();
 
         if (playerTileChanged && mDbWorker != nullptr)
-            mDbWorker->updateJobs(playerTile, mSettings.get().mMaxTilesNumber);
+            mDbWorker->update(playerTile);
     }
 
     void AsyncNavMeshUpdater::wait(WaitConditionType waitConditionType, Loading::Listener* listener)
@@ -310,7 +406,7 @@ namespace DetourNavigator
         {
             const std::lock_guard<std::mutex> lock(mMutex);
             result.mJobs = mJobs.size();
-            result.mWaiting = mWaiting.size();
+            result.mWaiting = mWaiting.getStats();
             result.mPushed = mPushed.size();
         }
         result.mProcessing = mProcessingTiles.lockConst()->size();
@@ -332,7 +428,8 @@ namespace DetourNavigator
                 if (JobIt job = getNextJob(); job != mJobs.end())
                 {
                     const JobStatus status = processJob(*job);
-                    Log(Debug::Debug) << "Processed job " << job->mId << " with status=" << status;
+                    Log(Debug::Debug) << "Processed job " << job->mId << " with status=" << status
+                                      << " changeType=" << job->mChangeType;
                     switch (status)
                     {
                         case JobStatus::Done:
@@ -366,7 +463,9 @@ namespace DetourNavigator
 
     JobStatus AsyncNavMeshUpdater::processJob(Job& job)
     {
-        Log(Debug::Debug) << "Processing job " << job.mId << " by thread=" << std::this_thread::get_id();
+        Log(Debug::Debug) << "Processing job " << job.mId << " for agent=(" << job.mAgentBounds << ")"
+                          << " changedTile=(" << job.mChangedTile << ")"
+                          << " changeType=" << job.mChangeType << " by thread=" << std::this_thread::get_id();
 
         const auto navMeshCacheItem = job.mNavMeshCacheItem.lock();
 
@@ -378,6 +477,7 @@ namespace DetourNavigator
         if (!shouldAddTile(job.mChangedTile, playerTile, mSettings.get().mMaxTilesNumber))
         {
             Log(Debug::Debug) << "Ignore add tile by job " << job.mId << ": too far from player";
+            job.mChangeType = ChangeType::remove;
             navMeshCacheItem->lock()->removeTile(job.mChangedTile);
             return JobStatus::Done;
         }
@@ -545,9 +645,8 @@ namespace DetourNavigator
 
         bool shouldStop = false;
         const auto hasJob = [&] {
-            shouldStop = mShouldStop;
-            return shouldStop
-                || (!mWaiting.empty() && mWaiting.front()->mProcessTime <= std::chrono::steady_clock::now());
+            shouldStop = mShouldStop.load();
+            return shouldStop || mWaiting.hasJob();
         };
 
         if (!mHasJob.wait_for(lock, std::chrono::milliseconds(10), hasJob))
@@ -560,9 +659,15 @@ namespace DetourNavigator
         if (shouldStop)
             return mJobs.end();
 
-        const JobIt job = mWaiting.front();
+        const TilePosition playerTile = *mPlayerTile.lockConst();
+
+        JobIt job = mJobs.end();
 
-        mWaiting.pop_front();
+        if (const std::optional<JobIt> nextJob = mWaiting.pop(playerTile))
+            job = *nextJob;
+
+        if (job == mJobs.end())
+            return job;
 
         Log(Debug::Debug) << "Pop job " << job->mId << " by thread=" << std::this_thread::get_id();
 
@@ -571,9 +676,9 @@ namespace DetourNavigator
 
         if (!lockTile(job->mId, job->mAgentBounds, job->mChangedTile))
         {
-            Log(Debug::Debug) << "Failed to lock tile by job " << job->mId << " try=" << job->mTryNumber;
-            ++job->mTryNumber;
-            insertPrioritizedJob(job, mWaiting);
+            Log(Debug::Debug) << "Failed to lock tile by job " << job->mId;
+            job->mProcessTime = std::chrono::steady_clock::now() + mSettings.get().mMinUpdateInterval;
+            mWaiting.push(job);
             return mJobs.end();
         }
 
@@ -653,7 +758,7 @@ namespace DetourNavigator
     {
         Log(Debug::Debug) << "Enqueueing job " << job->mId << " by thread=" << std::this_thread::get_id();
         const std::lock_guard lock(mMutex);
-        insertPrioritizedJob(job, mWaiting);
+        mWaiting.push(job);
         mHasJob.notify_all();
     }
 
@@ -667,40 +772,47 @@ namespace DetourNavigator
     void DbJobQueue::push(JobIt job)
     {
         const std::lock_guard lock(mMutex);
-        insertPrioritizedDbJob(job, mJobs);
         if (isWritingDbJob(*job))
-            ++mWritingJobs;
+            mWriting.push_back(job);
         else
-            ++mReadingJobs;
+            mReading.push(job);
         mHasJob.notify_all();
     }
 
     std::optional<JobIt> DbJobQueue::pop()
     {
         std::unique_lock lock(mMutex);
-        mHasJob.wait(lock, [&] { return mShouldStop || !mJobs.empty(); });
-        if (mJobs.empty())
+
+        const auto hasJob = [&] { return mShouldStop || mReading.size() > 0 || mWriting.size() > 0; };
+
+        mHasJob.wait(lock, hasJob);
+
+        if (mShouldStop)
             return std::nullopt;
-        const JobIt job = mJobs.front();
-        mJobs.pop_front();
-        if (isWritingDbJob(*job))
-            --mWritingJobs;
-        else
-            --mReadingJobs;
+
+        if (const std::optional<JobIt> job = mReading.pop(mPlayerTile))
+            return job;
+
+        if (mWriting.empty())
+            return std::nullopt;
+
+        const JobIt job = mWriting.front();
+        mWriting.pop_front();
+
         return job;
     }
 
-    void DbJobQueue::update(TilePosition playerTile, int maxTiles)
+    void DbJobQueue::update(TilePosition playerTile)
     {
         const std::lock_guard lock(mMutex);
-        updateJobs(mJobs, playerTile, maxTiles);
-        std::sort(mJobs.begin(), mJobs.end(), LessByJobDbPriority{});
+        mPlayerTile = playerTile;
     }
 
     void DbJobQueue::stop()
     {
         const std::lock_guard lock(mMutex);
-        mJobs.clear();
+        mReading.clear();
+        mWriting.clear();
         mShouldStop = true;
         mHasJob.notify_all();
     }
@@ -708,7 +820,10 @@ namespace DetourNavigator
     DbJobQueueStats DbJobQueue::getStats() const
     {
         const std::lock_guard lock(mMutex);
-        return DbJobQueueStats{ .mWritingJobs = mWritingJobs, .mReadingJobs = mReadingJobs };
+        return DbJobQueueStats{
+            .mReadingJobs = mReading.size(),
+            .mWritingJobs = mWriting.size(),
+        };
     }
 
     DbWorker::DbWorker(AsyncNavMeshUpdater& updater, std::unique_ptr<NavMeshDb>&& db, TileVersion version,
@@ -737,8 +852,10 @@ namespace DetourNavigator
 
     DbWorkerStats DbWorker::getStats() const
     {
-        return DbWorkerStats{ .mJobs = mQueue.getStats(),
-            .mGetTileCount = mGetTileCount.load(std::memory_order_relaxed) };
+        return DbWorkerStats{
+            .mJobs = mQueue.getStats(),
+            .mGetTileCount = mGetTileCount.load(std::memory_order_relaxed),
+        };
     }
 
     void DbWorker::stop()

components/detournavigator/asyncnavmeshupdater.hpp

@@ -14,6 +14,9 @@
 #include "tileposition.hpp"
 #include "waitconditiontype.hpp"
 
+#include <boost/geometry/geometries/point.hpp>
+#include <boost/geometry/index/rtree.hpp>
+
 #include <atomic>
 #include <chrono>
 #include <condition_variable>
@@ -49,11 +52,8 @@ namespace DetourNavigator
         const std::weak_ptr<GuardedNavMeshCacheItem> mNavMeshCacheItem;
         const std::string mWorldspace;
         const TilePosition mChangedTile;
-        const std::chrono::steady_clock::time_point mProcessTime;
-        unsigned mTryNumber = 0;
+        std::chrono::steady_clock::time_point mProcessTime;
         ChangeType mChangeType;
-        int mDistanceToPlayer;
-        const int mDistanceToOrigin;
         JobState mState = JobState::Initial;
         std::vector<std::byte> mInput;
         std::shared_ptr<RecastMesh> mRecastMesh;
@@ -61,12 +61,65 @@ namespace DetourNavigator
         std::unique_ptr<PreparedNavMeshData> mGeneratedNavMeshData;
 
         Job(const AgentBounds& agentBounds, std::weak_ptr<GuardedNavMeshCacheItem> navMeshCacheItem,
-            std::string_view worldspace, const TilePosition& changedTile, ChangeType changeType, int distanceToPlayer,
+            std::string_view worldspace, const TilePosition& changedTile, ChangeType changeType,
             std::chrono::steady_clock::time_point processTime);
     };
 
     using JobIt = std::list<Job>::iterator;
 
+    class SpatialJobQueue
+    {
+    public:
+        std::size_t size() const { return mSize; }
+
+        void clear();
+
+        void push(JobIt job);
+
+        std::optional<JobIt> pop(TilePosition playerTile);
+
+        void update(TilePosition playerTile, int maxTiles, std::vector<JobIt>& removing);
+
+    private:
+        using IndexPoint = boost::geometry::model::point<int, 2, boost::geometry::cs::cartesian>;
+        using UpdatingMap = std::map<TilePosition, std::deque<JobIt>>;
+        using IndexValue = std::pair<IndexPoint, UpdatingMap::iterator>;
+
+        std::size_t mSize = 0;
+        UpdatingMap mValues;
+        boost::geometry::index::rtree<IndexValue, boost::geometry::index::linear<4>> mIndex;
+    };
+
+    class JobQueue
+    {
+    public:
+        JobQueueStats getStats() const
+        {
+            return JobQueueStats{
+                .mRemoving = mRemoving.size(),
+                .mUpdating = mUpdating.size(),
+                .mDelayed = mDelayed.size(),
+            };
+        }
+
+        bool hasJob(std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now()) const;
+
+        void clear();
+
+        void push(JobIt job, std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now());
+
+        std::optional<JobIt> pop(
+            TilePosition playerTile, std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now());
+
+        void update(TilePosition playerTile, int maxTiles,
+            std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now());
+
+    private:
+        std::vector<JobIt> mRemoving;
+        SpatialJobQueue mUpdating;
+        std::deque<JobIt> mDelayed;
+    };
+
     enum class JobStatus
     {
         Done,
@@ -83,7 +136,7 @@ namespace DetourNavigator
 
         std::optional<JobIt> pop();
 
-        void update(TilePosition playerTile, int maxTiles);
+        void update(TilePosition playerTile);
 
         void stop();
 
@@ -92,10 +145,10 @@ namespace DetourNavigator
     private:
         mutable std::mutex mMutex;
         std::condition_variable mHasJob;
-        std::deque<JobIt> mJobs;
+        SpatialJobQueue mReading;
+        std::deque<JobIt> mWriting;
+        TilePosition mPlayerTile;
         bool mShouldStop = false;
-        std::size_t mWritingJobs = 0;
-        std::size_t mReadingJobs = 0;
     };
 
     class AsyncNavMeshUpdater;
@@ -112,7 +165,7 @@ namespace DetourNavigator
 
         void enqueueJob(JobIt job);
 
-        void updateJobs(TilePosition playerTile, int maxTiles) { mQueue.update(playerTile, maxTiles); }
+        void update(TilePosition playerTile) { mQueue.update(playerTile); }
 
         void stop();
 
@@ -169,7 +222,7 @@ namespace DetourNavigator
         std::condition_variable mDone;
         std::condition_variable mProcessed;
         std::list<Job> mJobs;
-        std::deque<JobIt> mWaiting;
+        JobQueue mWaiting;
         std::set<std::tuple<AgentBounds, TilePosition>> mPushed;
         Misc::ScopeGuarded<TilePosition> mPlayerTile;
         NavMeshTilesCache mNavMeshTilesCache;

components/detournavigator/changetype.hpp

@@ -6,15 +6,9 @@ namespace DetourNavigator
     enum class ChangeType
     {
         remove = 0,
-        mixed = 1,
-        add = 2,
-        update = 3,
+        add = 1,
+        update = 2,
     };
-
-    inline ChangeType addChangeType(const ChangeType current, const ChangeType add)
-    {
-        return current == add ? current : ChangeType::mixed;
-    }
 }
 
 #endif

components/detournavigator/debug.cpp

@@ -184,8 +184,6 @@ namespace DetourNavigator
         {
             case ChangeType::remove:
                 return stream << "ChangeType::remove";
-            case ChangeType::mixed:
-                return stream << "ChangeType::mixed";
             case ChangeType::add:
                 return stream << "ChangeType::add";
             case ChangeType::update:

components/detournavigator/navmeshmanager.cpp

@@ -188,7 +188,7 @@ namespace DetourNavigator
                 if (shouldAdd && !presentInNavMesh)
                     tilesToPost.emplace(tile, locked->isEmptyTile(tile) ? ChangeType::update : ChangeType::add);
                 else if (!shouldAdd && presentInNavMesh)
-                    tilesToPost.emplace(tile, ChangeType::mixed);
+                    tilesToPost.emplace(tile, ChangeType::remove);
             });
             locked->forEachTilePosition([&](const TilePosition& tile) {
                 if (!shouldAddTile(tile, playerTile, maxTiles))

components/detournavigator/stats.cpp

@@ -9,16 +9,18 @@ namespace DetourNavigator
         void reportStats(const AsyncNavMeshUpdaterStats& stats, unsigned int frameNumber, osg::Stats& out)
         {
             out.setAttribute(frameNumber, "NavMesh Jobs", static_cast<double>(stats.mJobs));
-            out.setAttribute(frameNumber, "NavMesh Waiting", static_cast<double>(stats.mWaiting));
+            out.setAttribute(frameNumber, "NavMesh Removing", static_cast<double>(stats.mWaiting.mRemoving));
+            out.setAttribute(frameNumber, "NavMesh Updating", static_cast<double>(stats.mWaiting.mUpdating));
+            out.setAttribute(frameNumber, "NavMesh Delayed", static_cast<double>(stats.mWaiting.mDelayed));
             out.setAttribute(frameNumber, "NavMesh Pushed", static_cast<double>(stats.mPushed));
             out.setAttribute(frameNumber, "NavMesh Processing", static_cast<double>(stats.mProcessing));
 
             if (stats.mDb.has_value())
             {
-                out.setAttribute(
-                    frameNumber, "NavMesh DbJobs Write", static_cast<double>(stats.mDb->mJobs.mWritingJobs));
                 out.setAttribute(
                     frameNumber, "NavMesh DbJobs Read", static_cast<double>(stats.mDb->mJobs.mReadingJobs));
+                out.setAttribute(
+                    frameNumber, "NavMesh DbJobs Write", static_cast<double>(stats.mDb->mJobs.mWritingJobs));
 
                 out.setAttribute(frameNumber, "NavMesh DbCache Get", static_cast<double>(stats.mDb->mGetTileCount));
                 out.setAttribute(frameNumber, "NavMesh DbCache Hit", static_cast<double>(stats.mDbGetTileHits));

components/detournavigator/stats.hpp

@@ -11,10 +11,17 @@ namespace osg
 
 namespace DetourNavigator
 {
+    struct JobQueueStats
+    {
+        std::size_t mRemoving = 0;
+        std::size_t mUpdating = 0;
+        std::size_t mDelayed = 0;
+    };
+
     struct DbJobQueueStats
     {
-        std::size_t mWritingJobs = 0;
         std::size_t mReadingJobs = 0;
+        std::size_t mWritingJobs = 0;
     };
 
     struct DbWorkerStats
@@ -35,7 +42,7 @@ namespace DetourNavigator
     struct AsyncNavMeshUpdaterStats
     {
         std::size_t mJobs = 0;
-        std::size_t mWaiting = 0;
+        JobQueueStats mWaiting;
         std::size_t mPushed = 0;
         std::size_t mProcessing = 0;
         std::size_t mDbGetTileHits = 0;

components/detournavigator/tilecachedrecastmeshmanager.cpp

@@ -10,7 +10,6 @@
 
 #include <boost/geometry/geometry.hpp>
 
-#include <algorithm>
 #include <limits>
 #include <vector>
 
@@ -416,7 +415,7 @@ namespace DetourNavigator
         if (tile == mChangedTiles.end())
             mChangedTiles.emplace(tilePosition, changeType);
         else
-            tile->second = addChangeType(tile->second, changeType);
+            tile->second = changeType == ChangeType::remove ? changeType : tile->second;
     }
 
     std::map<osg::Vec2i, ChangeType> TileCachedRecastMeshManager::takeChangedTiles(const UpdateGuard* guard)

components/resource/stats.cpp

@@ -49,6 +49,8 @@ namespace Resource
 
         std::vector<std::string> generateAllStatNames()
         {
+            constexpr std::size_t itemsPerPage = 24;
+
             constexpr std::string_view firstPage[] = {
                 "FrameNumber",
                 "",
@@ -76,6 +78,8 @@ namespace Resource
                 "",
             };
 
+            static_assert(std::size(firstPage) == itemsPerPage);
+
             constexpr std::string_view caches[] = {
                 "Node",
                 "Shape",
@@ -100,7 +104,9 @@ namespace Resource
 
             constexpr std::string_view navMesh[] = {
                 "NavMesh Jobs",
-                "NavMesh Waiting",
+                "NavMesh Removing",
+                "NavMesh Updating",
+                "NavMesh Delayed",
                 "NavMesh Pushed",
                 "NavMesh Processing",
                 "NavMesh DbJobs Write",
@@ -129,7 +135,8 @@ namespace Resource
             for (std::string_view name : cellPreloader)
                 statNames.emplace_back(name);
 
-            statNames.emplace_back();
+            while (statNames.size() % itemsPerPage != 0)
+                statNames.emplace_back();
 
             for (std::string_view name : navMesh)
                 statNames.emplace_back(name);