From 50f4471750d934c238244ae5e408d69bf681dc0c Mon Sep 17 00:00:00 2001 From: elsid Date: Sat, 6 Apr 2024 01:10:48 +0200 Subject: [PATCH] Use R-tree for dynamic priority of navmesh async job --- .../detournavigator/asyncnavmeshupdater.cpp | 271 +++++++++++++++- .../detournavigator/asyncnavmeshupdater.cpp | 307 ++++++++++++------ .../detournavigator/asyncnavmeshupdater.hpp | 75 ++++- components/detournavigator/changetype.hpp | 10 +- components/detournavigator/debug.cpp | 2 - components/detournavigator/navmeshmanager.cpp | 2 +- components/detournavigator/stats.cpp | 8 +- components/detournavigator/stats.hpp | 11 +- .../tilecachedrecastmeshmanager.cpp | 3 +- components/resource/stats.cpp | 11 +- 10 files changed, 573 insertions(+), 127 deletions(-) diff --git a/apps/openmw_test_suite/detournavigator/asyncnavmeshupdater.cpp b/apps/openmw_test_suite/detournavigator/asyncnavmeshupdater.cpp index bc1288f5f6..51ab37b123 100644 --- a/apps/openmw_test_suite/detournavigator/asyncnavmeshupdater.cpp +++ b/apps/openmw_test_suite/detournavigator/asyncnavmeshupdater.cpp @@ -267,7 +267,6 @@ namespace updater.wait(WaitConditionType::allJobsDone, &mListener); updater.stop(); const std::set 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(1, mSettings); + + std::map 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 mNavMeshCacheItemPtr; + const std::weak_ptr 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 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 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 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 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 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 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 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 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 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 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 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); + } } diff --git a/components/detournavigator/asyncnavmeshupdater.cpp b/components/detournavigator/asyncnavmeshupdater.cpp index bb04c2af07..fe6d0625f5 100644 --- a/components/detournavigator/asyncnavmeshupdater.cpp +++ b/components/detournavigator/asyncnavmeshupdater.cpp @@ -16,8 +16,9 @@ #include +#include + #include -#include #include #include #include @@ -49,40 +50,6 @@ namespace DetourNavigator return false; } - auto getPriority(const Job& job) noexcept - { - return std::make_tuple(-static_cast>(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& 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>(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& 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& 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 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()); + mIndex.insert(IndexValue(IndexPoint(job->mChangedTile.x(), job->mChangedTile.y()), it)); + } + + it->second.push_back(job); + + ++mSize; + } + + std::optional 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& 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& 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 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 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&& 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 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 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 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 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&& 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() diff --git a/components/detournavigator/asyncnavmeshupdater.hpp b/components/detournavigator/asyncnavmeshupdater.hpp index 09119556cd..f3d624a8a6 100644 --- a/components/detournavigator/asyncnavmeshupdater.hpp +++ b/components/detournavigator/asyncnavmeshupdater.hpp @@ -14,6 +14,9 @@ #include "tileposition.hpp" #include "waitconditiontype.hpp" +#include +#include + #include #include #include @@ -49,11 +52,8 @@ namespace DetourNavigator const std::weak_ptr 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 mInput; std::shared_ptr mRecastMesh; @@ -61,12 +61,65 @@ namespace DetourNavigator std::unique_ptr mGeneratedNavMeshData; Job(const AgentBounds& agentBounds, std::weak_ptr 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::iterator; + class SpatialJobQueue + { + public: + std::size_t size() const { return mSize; } + + void clear(); + + void push(JobIt job); + + std::optional pop(TilePosition playerTile); + + void update(TilePosition playerTile, int maxTiles, std::vector& removing); + + private: + using IndexPoint = boost::geometry::model::point; + using UpdatingMap = std::map>; + using IndexValue = std::pair; + + std::size_t mSize = 0; + UpdatingMap mValues; + boost::geometry::index::rtree> 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 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 mRemoving; + SpatialJobQueue mUpdating; + std::deque mDelayed; + }; + enum class JobStatus { Done, @@ -83,7 +136,7 @@ namespace DetourNavigator std::optional 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 mJobs; + SpatialJobQueue mReading; + std::deque 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 mJobs; - std::deque mWaiting; + JobQueue mWaiting; std::set> mPushed; Misc::ScopeGuarded mPlayerTile; NavMeshTilesCache mNavMeshTilesCache; diff --git a/components/detournavigator/changetype.hpp b/components/detournavigator/changetype.hpp index e6d0bce0a9..63a43e88fb 100644 --- a/components/detournavigator/changetype.hpp +++ b/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 diff --git a/components/detournavigator/debug.cpp b/components/detournavigator/debug.cpp index 4dcd5e9857..5ce1464bdd 100644 --- a/components/detournavigator/debug.cpp +++ b/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: diff --git a/components/detournavigator/navmeshmanager.cpp b/components/detournavigator/navmeshmanager.cpp index ea20f8bc34..3b62866ed7 100644 --- a/components/detournavigator/navmeshmanager.cpp +++ b/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)) diff --git a/components/detournavigator/stats.cpp b/components/detournavigator/stats.cpp index 1d7dcac553..da56f91a38 100644 --- a/components/detournavigator/stats.cpp +++ b/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(stats.mJobs)); - out.setAttribute(frameNumber, "NavMesh Waiting", static_cast(stats.mWaiting)); + out.setAttribute(frameNumber, "NavMesh Removing", static_cast(stats.mWaiting.mRemoving)); + out.setAttribute(frameNumber, "NavMesh Updating", static_cast(stats.mWaiting.mUpdating)); + out.setAttribute(frameNumber, "NavMesh Delayed", static_cast(stats.mWaiting.mDelayed)); out.setAttribute(frameNumber, "NavMesh Pushed", static_cast(stats.mPushed)); out.setAttribute(frameNumber, "NavMesh Processing", static_cast(stats.mProcessing)); if (stats.mDb.has_value()) { - out.setAttribute( - frameNumber, "NavMesh DbJobs Write", static_cast(stats.mDb->mJobs.mWritingJobs)); out.setAttribute( frameNumber, "NavMesh DbJobs Read", static_cast(stats.mDb->mJobs.mReadingJobs)); + out.setAttribute( + frameNumber, "NavMesh DbJobs Write", static_cast(stats.mDb->mJobs.mWritingJobs)); out.setAttribute(frameNumber, "NavMesh DbCache Get", static_cast(stats.mDb->mGetTileCount)); out.setAttribute(frameNumber, "NavMesh DbCache Hit", static_cast(stats.mDbGetTileHits)); diff --git a/components/detournavigator/stats.hpp b/components/detournavigator/stats.hpp index c644f1db87..0b62b9e669 100644 --- a/components/detournavigator/stats.hpp +++ b/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; diff --git a/components/detournavigator/tilecachedrecastmeshmanager.cpp b/components/detournavigator/tilecachedrecastmeshmanager.cpp index 0bab808300..3e3927bf65 100644 --- a/components/detournavigator/tilecachedrecastmeshmanager.cpp +++ b/components/detournavigator/tilecachedrecastmeshmanager.cpp @@ -10,7 +10,6 @@ #include -#include #include #include @@ -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 TileCachedRecastMeshManager::takeChangedTiles(const UpdateGuard* guard) diff --git a/components/resource/stats.cpp b/components/resource/stats.cpp index 65b009deff..9bb90635d1 100644 --- a/components/resource/stats.cpp +++ b/components/resource/stats.cpp @@ -49,6 +49,8 @@ namespace Resource std::vector 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);