Limit and filter navmesh input (#5858)

apps/navmeshtool/navmesh.cpp

@@ -35,6 +35,7 @@
 #include <string_view>
 #include <utility>
 #include <vector>
+#include <random>
 
 namespace NavMeshTool
 {
@@ -180,26 +181,36 @@ namespace NavMeshTool
         SceneUtil::WorkQueue workQueue(threadsNumber);
         auto navMeshTileConsumer = std::make_shared<NavMeshTileConsumer>(std::move(db));
         std::size_t tiles = 0;
+        std::mt19937_64 random;
 
         for (const std::unique_ptr<WorldspaceNavMeshInput>& input : data.mNavMeshInputs)
         {
+            std::vector<TilePosition> worldspaceTiles;
+
             DetourNavigator::getTilesPositions(
-                Misc::Convert::toOsg(input->mAabb.m_min), Misc::Convert::toOsg(input->mAabb.m_max), settings.mRecast,
-                [&] (const TilePosition& tilePosition)
-                {
-                    workQueue.addWorkItem(new GenerateNavMeshTile(
-                        input->mWorldspace,
-                        tilePosition,
-                        RecastMeshProvider(input->mTileCachedRecastMeshManager),
-                        agentHalfExtents,
-                        settings,
-                        navMeshTileConsumer
-                    ));
-
-                    ++tiles;
-                });
+                DetourNavigator::makeTilesPositionsRange(
+                    Misc::Convert::toOsg(input->mAabb.m_min),
+                    Misc::Convert::toOsg(input->mAabb.m_max),
+                    settings.mRecast
+                ),
+                [&] (const TilePosition& tilePosition) { worldspaceTiles.push_back(tilePosition); }
+            );
+
+            tiles += worldspaceTiles.size();
 
             navMeshTileConsumer->mExpected = tiles;
+
+            std::shuffle(worldspaceTiles.begin(), worldspaceTiles.end(), random);
+
+            for (const TilePosition& tilePosition : worldspaceTiles)
+                workQueue.addWorkItem(new GenerateNavMeshTile(
+                    input->mWorldspace,
+                    tilePosition,
+                    RecastMeshProvider(input->mTileCachedRecastMeshManager),
+                    agentHalfExtents,
+                    settings,
+                    navMeshTileConsumer
+                ));
         }
 
         navMeshTileConsumer->wait();

apps/openmw/mwworld/scene.cpp

@@ -511,8 +511,7 @@ namespace MWWorld
         if (mCurrentCell == nullptr)
             return;
 
-        const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr();
-        mNavigator.updatePlayerPosition(player.getRefData().getPosition().asVec3());
+        mNavigator.updatePlayerPosition(pos);
 
         if (!mCurrentCell->isExterior())
             return;
@@ -823,6 +822,8 @@ namespace MWWorld
 
         loadingListener->setProgressRange(cell->count());
 
+        mNavigator.updatePlayerPosition(position.asVec3());
+
         // Load cell.
         mPagedRefs.clear();
         loadCell(cell, loadingListener, changeEvent);
@@ -856,6 +857,8 @@ namespace MWWorld
         if (changeEvent)
             MWBase::Environment::get().getWindowManager()->fadeScreenOut(0.5);
 
+        mNavigator.updatePlayerPosition(position.asVec3());
+
         changeCellGrid(position.asVec3(), x, y, changeEvent);
 
         CellStore* current = MWBase::Environment::get().getWorld()->getExterior(x, y);

apps/openmw_test_suite/detournavigator/gettilespositions.cpp

@@ -1,5 +1,6 @@
 #include <components/detournavigator/gettilespositions.hpp>
 #include <components/detournavigator/debug.hpp>
+#include <components/detournavigator/settings.hpp>
 
 #include <gtest/gtest.h>
 #include <gmock/gmock.h>
@@ -36,35 +37,35 @@ namespace
 
     TEST_F(DetourNavigatorGetTilesPositionsTest, for_object_in_single_tile_should_return_one_tile)
     {
-        getTilesPositions(osg::Vec3f(2, 2, 0), osg::Vec3f(31, 31, 1), mSettings, mCollect);
+        getTilesPositions(makeTilesPositionsRange(osg::Vec3f(2, 2, 0), osg::Vec3f(31, 31, 1), mSettings), mCollect);
 
         EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0)));
     }
 
     TEST_F(DetourNavigatorGetTilesPositionsTest, for_object_with_x_bounds_in_two_tiles_should_return_two_tiles)
     {
-        getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(32, 31, 1), mSettings, mCollect);
+        getTilesPositions(makeTilesPositionsRange(osg::Vec3f(0, 0, 0), osg::Vec3f(32, 31, 1), mSettings), mCollect);
 
         EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0), TilePosition(1, 0)));
     }
 
     TEST_F(DetourNavigatorGetTilesPositionsTest, for_object_with_y_bounds_in_two_tiles_should_return_two_tiles)
     {
-        getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(31, 32, 1), mSettings, mCollect);
+        getTilesPositions(makeTilesPositionsRange(osg::Vec3f(0, 0, 0), osg::Vec3f(31, 32, 1), mSettings), mCollect);
 
         EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0), TilePosition(0, 1)));
     }
 
     TEST_F(DetourNavigatorGetTilesPositionsTest, tiling_works_only_for_x_and_y_coordinates)
     {
-        getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(31, 31, 32), mSettings, mCollect);
+        getTilesPositions(makeTilesPositionsRange(osg::Vec3f(0, 0, 0), osg::Vec3f(31, 31, 32), mSettings), mCollect);
 
         EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0)));
     }
 
     TEST_F(DetourNavigatorGetTilesPositionsTest, tiling_should_work_with_negative_coordinates)
     {
-        getTilesPositions(osg::Vec3f(-31, -31, 0), osg::Vec3f(31, 31, 1), mSettings, mCollect);
+        getTilesPositions(makeTilesPositionsRange(osg::Vec3f(-31, -31, 0), osg::Vec3f(31, 31, 1), mSettings), mCollect);
 
         EXPECT_THAT(mTilesPositions, ElementsAre(
             TilePosition(-1, -1),
@@ -78,7 +79,7 @@ namespace
     {
         mSettings.mBorderSize = 1;
 
-        getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(31.5, 31.5, 1), mSettings, mCollect);
+        getTilesPositions(makeTilesPositionsRange(osg::Vec3f(0, 0, 0), osg::Vec3f(31.5, 31.5, 1), mSettings), mCollect);
 
         EXPECT_THAT(mTilesPositions, ElementsAre(
             TilePosition(-1, -1),
@@ -97,7 +98,7 @@ namespace
     {
         mSettings.mRecastScaleFactor = 0.5;
 
-        getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(32, 32, 1), mSettings, mCollect);
+        getTilesPositions(makeTilesPositionsRange(osg::Vec3f(0, 0, 0), osg::Vec3f(32, 32, 1), mSettings), mCollect);
 
         EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0)));
     }

apps/openmw_test_suite/detournavigator/tilecachedrecastmeshmanager.cpp

@@ -90,6 +90,10 @@ namespace
         const btBoxShape boxShape(btVector3(20, 20, 100));
         const btTransform transform(btMatrix3x3::getIdentity(), btVector3(getTileSize(mSettings) / mSettings.mRecastScaleFactor, 0, 0));
         const CollisionShape shape(mInstance, boxShape, mObjectTransform);
+        TileBounds bounds;
+        bounds.mMin = osg::Vec2f(-1000, -1000);
+        bounds.mMax = osg::Vec2f(1000, 1000);
+        manager.setBounds(bounds);
         manager.addObject(ObjectId(&boxShape), shape, transform, AreaType::AreaType_ground);
         EXPECT_TRUE(manager.updateObject(ObjectId(&boxShape), shape, btTransform::getIdentity(), AreaType::AreaType_ground,
                                          [&] (const auto& v) { onChangedTile(v); }));
@@ -137,6 +141,10 @@ namespace
     TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, get_mesh_for_moved_object_should_return_recast_mesh_for_each_used_tile)
     {
         TileCachedRecastMeshManager manager(mSettings);
+        TileBounds bounds;
+        bounds.mMin = osg::Vec2f(-1000, -1000);
+        bounds.mMax = osg::Vec2f(1000, 1000);
+        manager.setBounds(bounds);
         manager.setWorldspace("worldspace");
 
         const btBoxShape boxShape(btVector3(20, 20, 100));

components/CMakeLists.txt

@@ -208,6 +208,7 @@ add_component_dir(detournavigator
     serialization
     navmeshdbutils
     recast
+    gettilespositions
     )
 
 add_component_dir(loadinglistener

components/detournavigator/gettilespositions.cpp

@@ -0,0 +1,66 @@
+#include "gettilespositions.hpp"
+#include "settings.hpp"
+#include "settingsutils.hpp"
+#include "tileposition.hpp"
+#include "tilebounds.hpp"
+
+#include <components/misc/convert.hpp>
+
+#include <BulletCollision/CollisionShapes/btCollisionShape.h>
+
+namespace DetourNavigator
+{
+    TilesPositionsRange makeTilesPositionsRange(const osg::Vec3f& aabbMin, const osg::Vec3f& aabbMax,
+        const RecastSettings& settings)
+    {
+        osg::Vec3f min = toNavMeshCoordinates(settings, aabbMin);
+        osg::Vec3f max = toNavMeshCoordinates(settings, aabbMax);
+
+        const float border = getBorderSize(settings);
+        min -= osg::Vec3f(border, border, border);
+        max += osg::Vec3f(border, border, border);
+
+        TilePosition minTile = getTilePosition(settings, min);
+        TilePosition maxTile = getTilePosition(settings, max);
+
+        if (minTile.x() > maxTile.x())
+            std::swap(minTile.x(), maxTile.x());
+
+        if (minTile.y() > maxTile.y())
+            std::swap(minTile.y(), maxTile.y());
+
+        return {minTile, maxTile};
+    }
+
+    TilesPositionsRange makeTilesPositionsRange(const btCollisionShape& shape, const btTransform& transform,
+        const TileBounds& bounds, const RecastSettings& settings)
+    {
+        btVector3 aabbMin;
+        btVector3 aabbMax;
+        shape.getAabb(transform, aabbMin, aabbMax);
+        aabbMin.setX(std::max<btScalar>(aabbMin.x(), bounds.mMin.x()));
+        aabbMin.setY(std::max<btScalar>(aabbMin.y(), bounds.mMin.y()));
+        aabbMax.setX(std::min<btScalar>(aabbMax.x(), bounds.mMax.x()));
+        aabbMax.setY(std::min<btScalar>(aabbMax.y(), bounds.mMax.y()));
+        return makeTilesPositionsRange(Misc::Convert::toOsg(aabbMin), Misc::Convert::toOsg(aabbMax), settings);
+    }
+
+    TilesPositionsRange makeTilesPositionsRange(const int cellSize, const btVector3& shift,
+        const RecastSettings& settings)
+    {
+        using Misc::Convert::toOsg;
+
+        const int halfCellSize = cellSize / 2;
+        const btTransform transform(btMatrix3x3::getIdentity(), shift);
+        btVector3 aabbMin = transform(btVector3(-halfCellSize, -halfCellSize, 0));
+        btVector3 aabbMax = transform(btVector3(halfCellSize, halfCellSize, 0));
+
+        aabbMin.setX(std::min(aabbMin.x(), aabbMax.x()));
+        aabbMin.setY(std::min(aabbMin.y(), aabbMax.y()));
+
+        aabbMax.setX(std::max(aabbMin.x(), aabbMax.x()));
+        aabbMax.setY(std::max(aabbMin.y(), aabbMax.y()));
+
+        return makeTilesPositionsRange(toOsg(aabbMin), toOsg(aabbMax), settings);
+    }
+}

components/detournavigator/gettilespositions.hpp

@@ -1,72 +1,43 @@
 #ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_GETTILESPOSITIONS_H
 #define OPENMW_COMPONENTS_DETOURNAVIGATOR_GETTILESPOSITIONS_H
 
-#include "settings.hpp"
-#include "settingsutils.hpp"
+#include "tilebounds.hpp"
 #include "tileposition.hpp"
 
-#include <components/misc/convert.hpp>
+class btVector3;
+class btTransform;
+class btCollisionShape;
 
-#include <BulletCollision/CollisionShapes/btCollisionShape.h>
-
-#include <osg/Vec3f>
+namespace osg
+{
+    class Vec3f;
+}
 
 namespace DetourNavigator
 {
-    template <class Callback>
-    void getTilesPositions(const osg::Vec3f& aabbMin, const osg::Vec3f& aabbMax,
-        const RecastSettings& settings, Callback&& callback)
-    {
-        auto min = toNavMeshCoordinates(settings, aabbMin);
-        auto max = toNavMeshCoordinates(settings, aabbMax);
+    struct RecastSettings;
 
-        const auto border = getBorderSize(settings);
-        min -= osg::Vec3f(border, border, border);
-        max += osg::Vec3f(border, border, border);
+    struct TilesPositionsRange
+    {
+        TilePosition mMin;
+        TilePosition mMax;
+    };
 
-        auto minTile = getTilePosition(settings, min);
-        auto maxTile = getTilePosition(settings, max);
+    TilesPositionsRange makeTilesPositionsRange(const osg::Vec3f& aabbMin,
+        const osg::Vec3f& aabbMax, const RecastSettings& settings);
 
-        if (minTile.x() > maxTile.x())
-            std::swap(minTile.x(), maxTile.x());
+    TilesPositionsRange makeTilesPositionsRange(const btCollisionShape& shape,
+        const btTransform& transform, const TileBounds& bounds, const RecastSettings& settings);
 
-        if (minTile.y() > maxTile.y())
-            std::swap(minTile.y(), maxTile.y());
-
-        for (int tileX = minTile.x(); tileX <= maxTile.x(); ++tileX)
-            for (int tileY = minTile.y(); tileY <= maxTile.y(); ++tileY)
-                callback(TilePosition {tileX, tileY});
-    }
+    TilesPositionsRange makeTilesPositionsRange(const int cellSize, const btVector3& shift,
+        const RecastSettings& settings);
 
     template <class Callback>
-    void getTilesPositions(const btCollisionShape& shape, const btTransform& transform,
-        const RecastSettings& settings, Callback&& callback)
+    void getTilesPositions(const TilesPositionsRange& range, Callback&& callback)
     {
-        btVector3 aabbMin;
-        btVector3 aabbMax;
-        shape.getAabb(transform, aabbMin, aabbMax);
-
-        getTilesPositions(Misc::Convert::toOsg(aabbMin), Misc::Convert::toOsg(aabbMax), settings, std::forward<Callback>(callback));
-    }
-
-    template <class Callback>
-    void getTilesPositions(const int cellSize, const btVector3& shift,
-        const RecastSettings& settings, Callback&& callback)
-    {
-        using Misc::Convert::toOsg;
-
-        const auto halfCellSize = cellSize / 2;
-        const btTransform transform(btMatrix3x3::getIdentity(), shift);
-        auto aabbMin = transform(btVector3(-halfCellSize, -halfCellSize, 0));
-        auto aabbMax = transform(btVector3(halfCellSize, halfCellSize, 0));
-
-        aabbMin.setX(std::min(aabbMin.x(), aabbMax.x()));
-        aabbMin.setY(std::min(aabbMin.y(), aabbMax.y()));
-
-        aabbMax.setX(std::max(aabbMin.x(), aabbMax.x()));
-        aabbMax.setY(std::max(aabbMin.y(), aabbMax.y()));
-
-        getTilesPositions(toOsg(aabbMin), toOsg(aabbMax), settings, std::forward<Callback>(callback));
+        for (int tileX = range.mMin.x(); tileX <= range.mMax.x(); ++tileX)
+            for (int tileY = range.mMin.y(); tileY <= range.mMax.y(); ++tileY)
+                callback(TilePosition {tileX, tileY});
     }
 }
 

components/detournavigator/navigatorimpl.cpp

@@ -5,6 +5,7 @@
 #include <components/debug/debuglog.hpp>
 #include <components/esm/loadpgrd.hpp>
 #include <components/misc/coordinateconverter.hpp>
+#include <components/misc/convert.hpp>
 
 namespace DetourNavigator
 {

components/detournavigator/navmeshmanager.cpp

@@ -9,6 +9,7 @@
 
 #include <components/debug/debuglog.hpp>
 #include <components/bullethelpers/heightfield.hpp>
+#include <components/misc/convert.hpp>
 
 #include <DetourNavMesh.h>
 
@@ -41,6 +42,18 @@ namespace
 
 namespace DetourNavigator
 {
+    namespace
+    {
+        TileBounds makeBounds(const RecastSettings& settings, const osg::Vec2f& center, int maxTiles)
+        {
+            const float radius = fromNavMeshCoordinates(settings, std::ceil(std::sqrt(static_cast<float>(maxTiles) / osg::PIf) + 1) * getTileSize(settings));
+            TileBounds result;
+            result.mMin = center - osg::Vec2f(radius, radius);
+            result.mMax = center + osg::Vec2f(radius, radius);
+            return result;
+        }
+    }
+
     NavMeshManager::NavMeshManager(const Settings& settings, std::unique_ptr<NavMeshDb>&& db)
         : mSettings(settings)
         , mRecastMeshManager(settings.mRecast)
@@ -204,6 +217,7 @@ namespace DetourNavigator
                     }
             }
             const auto maxTiles = std::min(mSettings.mMaxTilesNumber, navMesh.getParams()->maxTiles);
+            mRecastMeshManager.setBounds(makeBounds(mSettings.mRecast, osg::Vec2f(playerPosition.x(), playerPosition.y()), maxTiles));
             mRecastMeshManager.forEachTile([&] (const TilePosition& tile, CachedRecastMeshManager& recastMeshManager)
             {
                 if (tilesToPost.count(tile))
@@ -262,7 +276,7 @@ namespace DetourNavigator
     void NavMeshManager::addChangedTiles(const btCollisionShape& shape, const btTransform& transform,
             const ChangeType changeType)
     {
-        getTilesPositions(shape, transform, mSettings.mRecast,
+        getTilesPositions(makeTilesPositionsRange(shape, transform, mRecastMeshManager.getBounds(), mSettings.mRecast),
             [&] (const TilePosition& v) { addChangedTile(v, changeType); });
     }
 
@@ -272,7 +286,7 @@ namespace DetourNavigator
         if (cellSize == std::numeric_limits<int>::max())
             return;
 
-        getTilesPositions(cellSize, shift, mSettings.mRecast,
+        getTilesPositions(makeTilesPositionsRange(cellSize, shift, mSettings.mRecast),
             [&] (const TilePosition& v) { addChangedTile(v, changeType); });
     }
 

components/detournavigator/recastmeshbuilder.cpp

@@ -20,6 +20,7 @@
 #include <array>
 #include <vector>
 #include <sstream>
+#include <cmath>
 
 namespace DetourNavigator
 {
@@ -40,6 +41,16 @@ namespace DetourNavigator
         {
             return static_cast<float>(cellSize) / (dataSize - 1);
         }
+
+        bool isNan(const RecastMeshTriangle& triangle)
+        {
+            for (std::size_t i = 0; i < 3; ++i)
+                if (std::isnan(triangle.mVertices[i].x())
+                    || std::isnan(triangle.mVertices[i].y())
+                    || std::isnan(triangle.mVertices[i].z()))
+                    return true;
+            return false;
+        }
     }
 
     Mesh makeMesh(std::vector<RecastMeshTriangle>&& triangles, const osg::Vec3f& shift)
@@ -264,6 +275,7 @@ namespace DetourNavigator
 
     std::shared_ptr<RecastMesh> RecastMeshBuilder::create(std::size_t generation, std::size_t revision) &&
     {
+        mTriangles.erase(std::remove_if(mTriangles.begin(), mTriangles.end(), isNan), mTriangles.end());
         std::sort(mTriangles.begin(), mTriangles.end());
         std::sort(mWater.begin(), mWater.end());
         Mesh mesh = makeMesh(std::move(mTriangles));

components/detournavigator/settingsutils.hpp

@@ -37,6 +37,11 @@ namespace DetourNavigator
         };
     }
 
+    inline float fromNavMeshCoordinates(const RecastSettings& settings, float value)
+    {
+        return value / settings.mRecastScaleFactor;
+    }
+
     inline osg::Vec3f fromNavMeshCoordinates(const RecastSettings& settings, osg::Vec3f position)
     {
         const auto factor = 1.0f / settings.mRecastScaleFactor;

components/detournavigator/tilecachedrecastmeshmanager.cpp

@@ -4,6 +4,7 @@
 #include "settingsutils.hpp"
 
 #include <components/debug/debuglog.hpp>
+#include <components/misc/convert.hpp>
 
 #include <algorithm>
 #include <vector>
@@ -14,6 +15,18 @@ namespace DetourNavigator
         : mSettings(settings)
     {}
 
+    TileBounds TileCachedRecastMeshManager::getBounds() const
+    {
+        const std::lock_guard lock(mMutex);
+        return mBounds;
+    }
+
+    void TileCachedRecastMeshManager::setBounds(const TileBounds& bounds)
+    {
+        const std::lock_guard lock(mMutex);
+        mBounds = bounds;
+    }
+
     std::string TileCachedRecastMeshManager::getWorldspace() const
     {
         const std::lock_guard lock(mMutex);
@@ -35,7 +48,8 @@ namespace DetourNavigator
         std::vector<TilePosition> tilesPositions;
         {
             const std::lock_guard lock(mMutex);
-            getTilesPositions(shape.getShape(), transform, mSettings, [&] (const TilePosition& tilePosition)
+            getTilesPositions(makeTilesPositionsRange(shape.getShape(), transform, mBounds, mSettings),
+                [&] (const TilePosition& tilePosition)
                 {
                     if (addTile(id, shape, transform, areaType, tilePosition, mTiles))
                         tilesPositions.push_back(tilePosition);
@@ -90,7 +104,8 @@ namespace DetourNavigator
         else
         {
             const btVector3 shift = Misc::Convert::toBullet(getWaterShift3d(cellPosition, cellSize, level));
-            getTilesPositions(cellSize, shift, mSettings, [&] (const TilePosition& tilePosition)
+            getTilesPositions(makeTilesPositionsRange(cellSize, shift, mSettings),
+                [&] (const TilePosition& tilePosition)
                 {
                     const std::lock_guard lock(mMutex);
                     auto tile = mTiles.find(tilePosition);
@@ -148,7 +163,8 @@ namespace DetourNavigator
 
         bool result = false;
 
-        getTilesPositions(cellSize, shift, mSettings, [&] (const TilePosition& tilePosition)
+        getTilesPositions(makeTilesPositionsRange(cellSize, shift, mSettings),
+            [&] (const TilePosition& tilePosition)
             {
                 const std::lock_guard lock(mMutex);
                 auto tile = mTiles.find(tilePosition);

components/detournavigator/tilecachedrecastmeshmanager.hpp

@@ -20,6 +20,10 @@ namespace DetourNavigator
     public:
         explicit TileCachedRecastMeshManager(const RecastSettings& settings);
 
+        TileBounds getBounds() const;
+
+        void setBounds(const TileBounds& bounds);
+
         std::string getWorldspace() const;
 
         void setWorldspace(std::string_view worldspace);
@@ -57,7 +61,7 @@ namespace DetourNavigator
                         changed = true;
                     }
                 };
-                getTilesPositions(shape.getShape(), transform, mSettings, onTilePosition);
+                getTilesPositions(makeTilesPositionsRange(shape.getShape(), transform, mBounds, mSettings), onTilePosition);
                 std::sort(newTiles.begin(), newTiles.end());
                 for (const auto& tile : currentTiles)
                 {
@@ -109,6 +113,7 @@ namespace DetourNavigator
 
         const RecastSettings& mSettings;
         mutable std::mutex mMutex;
+        TileBounds mBounds;
         std::string mWorldspace;
         TilesMap mTiles;
         std::unordered_map<ObjectId, std::vector<TilePosition>> mObjectsTilesPositions;