Merge branch 'recast_mesh_heightfield' into 'master'

Reduce navmesh cache size by special handling for heightfields

See merge request OpenMW/openmw!1032

apps/benchmarks/detournavigator/navmeshtilescache.cpp

@@ -1,6 +1,7 @@
 #include <benchmark/benchmark.h>
 
 #include <components/detournavigator/navmeshtilescache.hpp>
+#include <components/esm/loadland.hpp>
 
 #include <algorithm>
 #include <random>
@@ -30,6 +31,14 @@ namespace
         return TilePosition(distribution(random), distribution(random));
     }
 
+    template <typename Random>
+    TileBounds generateTileBounds(Random& random)
+    {
+        std::uniform_real_distribution<float> distribution(0.0, 1.0);
+        const osg::Vec2f min(distribution(random), distribution(random));
+        return TileBounds {min, min + osg::Vec2f(1.0, 1.0)};
+    }
+
     template <typename Random>
     osg::Vec3f generateAgentHalfExtents(float min, float max, Random& random)
     {
@@ -80,13 +89,57 @@ namespace
     template <typename OutputIterator, typename Random>
     void generateWater(OutputIterator out, std::size_t count, Random& random)
     {
-        std::uniform_real_distribution<btScalar> distribution(0.0, 1.0);
+        std::uniform_real_distribution<float> distribution(0.0, 1.0);
         std::generate_n(out, count, [&] {
-            const btVector3 shift(distribution(random), distribution(random), distribution(random));
+            const osg::Vec3f shift(distribution(random), distribution(random), distribution(random));
-            return RecastMesh::Water {1, btTransform(btMatrix3x3::getIdentity(), shift)};
+            return Cell {1, shift};
         });
     }
 
+    template <class Random>
+    Mesh generateMesh(std::size_t triangles, Random& random)
+    {
+        std::uniform_real_distribution<float> distribution(0.0, 1.0);
+        std::vector<float> vertices;
+        std::vector<int> indices;
+        std::vector<AreaType> areaTypes;
+        if (distribution(random) < 0.939)
+        {
+            generateVertices(std::back_inserter(vertices), triangles * 2.467, random);
+            generateIndices(std::back_inserter(indices), static_cast<int>(vertices.size() / 3) - 1, vertices.size() * 1.279, random);
+            generateAreaTypes(std::back_inserter(areaTypes), indices.size() / 3, random);
+        }
+        return Mesh(std::move(indices), std::move(vertices), std::move(areaTypes));
+    }
+
+    template <class Random>
+    Heightfield generateHeightfield(Random& random)
+    {
+        std::uniform_real_distribution<float> distribution(0.0, 1.0);
+        Heightfield result;
+        result.mBounds = generateTileBounds(random);
+        result.mMinHeight = distribution(random);
+        result.mMaxHeight = result.mMinHeight + 1.0;
+        result.mShift = osg::Vec3f(distribution(random), distribution(random), distribution(random));
+        result.mScale = distribution(random);
+        result.mLength = static_cast<std::uint8_t>(ESM::Land::LAND_SIZE);
+        std::generate_n(std::back_inserter(result.mHeights), ESM::Land::LAND_NUM_VERTS, [&]
+        {
+            return distribution(random);
+        });
+        return result;
+    }
+
+    template <class Random>
+    FlatHeightfield generateFlatHeightfield(Random& random)
+    {
+        std::uniform_real_distribution<float> distribution(0.0, 1.0);
+        FlatHeightfield result;
+        result.mBounds = generateTileBounds(random);
+        result.mHeight = distribution(random);
+        return result;
+    }
+
     template <class Random>
     Key generateKey(std::size_t triangles, Random& random)
     {
@@ -94,20 +147,15 @@ namespace
         const TilePosition tilePosition = generateTilePosition(10000, random);
         const std::size_t generation = std::uniform_int_distribution<std::size_t>(0, 100)(random);
         const std::size_t revision = std::uniform_int_distribution<std::size_t>(0, 10000)(random);
-        std::vector<float> vertices;
+        Mesh mesh = generateMesh(triangles, random);
-        generateVertices(std::back_inserter(vertices), triangles * 1.98, random);
+        std::vector<Cell> water;
-        std::vector<int> indices;
+        generateWater(std::back_inserter(water), 1, random);
-        generateIndices(std::back_inserter(indices), static_cast<int>(vertices.size() / 3) - 1, vertices.size() * 1.53, random);
+        RecastMesh recastMesh(generation, revision, std::move(mesh), std::move(water),
-        std::vector<AreaType> areaTypes;
+                              {generateHeightfield(random)}, {generateFlatHeightfield(random)});
-        generateAreaTypes(std::back_inserter(areaTypes), indices.size() / 3, random);
-        std::vector<RecastMesh::Water> water;
-        generateWater(std::back_inserter(water), 2, random);
-        RecastMesh recastMesh(generation, revision, std::move(indices), std::move(vertices),
-                              std::move(areaTypes), std::move(water));
         return Key {agentHalfExtents, tilePosition, std::move(recastMesh)};
     }
 
-    constexpr std::size_t trianglesPerTile = 310;
+    constexpr std::size_t trianglesPerTile = 239;
 
     template <typename OutputIterator, typename Random>
     void generateKeys(OutputIterator out, std::size_t count, Random& random)

apps/openmw/mwworld/scene.cpp

@@ -20,6 +20,7 @@
 #include <components/detournavigator/navigator.hpp>
 #include <components/detournavigator/debug.hpp>
 #include <components/misc/convert.hpp>
+#include <components/detournavigator/heightfieldshape.hpp>
 
 #include "../mwbase/environment.hpp"
 #include "../mwbase/world.hpp"
@@ -370,7 +371,6 @@ namespace MWWorld
         if (!test)
             Log(Debug::Info) << "Deactivate cell " << cell->getCell()->getDescription();
 
-        const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
         ListAndResetObjectsVisitor visitor;
 
         cell->forEach(visitor);
@@ -379,13 +379,13 @@ namespace MWWorld
         {
             if (const auto object = mPhysics->getObject(ptr))
             {
-                navigator->removeObject(DetourNavigator::ObjectId(object));
+                mNavigator.removeObject(DetourNavigator::ObjectId(object));
                 if (object->isAnimated())
                     mPhysics->remove(ptr);
             }
             else if (mPhysics->getActor(ptr))
             {
-                navigator->removeAgent(world->getPathfindingHalfExtents(ptr));
+                mNavigator.removeAgent(world->getPathfindingHalfExtents(ptr));
                 mRendering.removeActorPath(ptr);
                 mPhysics->remove(ptr);
             }
@@ -398,17 +398,17 @@ namespace MWWorld
         if (cell->getCell()->isExterior())
         {
             if (const auto heightField = mPhysics->getHeightField(cellX, cellY))
-                navigator->removeObject(DetourNavigator::ObjectId(heightField));
+                mNavigator.removeObject(DetourNavigator::ObjectId(heightField));
         }
 
         if (cell->getCell()->hasWater())
-            navigator->removeWater(osg::Vec2i(cellX, cellY));
+            mNavigator.removeWater(osg::Vec2i(cellX, cellY));
 
         if (const auto pathgrid = world->getStore().get<ESM::Pathgrid>().search(*cell->getCell()))
-            navigator->removePathgrid(*pathgrid);
+            mNavigator.removePathgrid(*pathgrid);
 
         const auto player = world->getPlayerPtr();
-        navigator->update(player.getRefData().getPosition().asVec3());
+        mNavigator.update(player.getRefData().getPosition().asVec3());
 
         MWBase::Environment::get().getMechanicsManager()->drop (cell);
 
@@ -423,6 +423,8 @@ namespace MWWorld
 
     void Scene::activateCell (CellStore *cell, Loading::Listener* loadingListener, bool respawn, bool test)
     {
+        using DetourNavigator::HeightfieldShape;
+
         assert(mActiveCells.find(cell) == mActiveCells.end());
         assert(mInactiveCells.find(cell) != mInactiveCells.end());
         mActiveCells.insert(cell);
@@ -433,7 +435,6 @@ namespace MWWorld
             Log(Debug::Info) << "Loading cell " << cell->getCell()->getDescription();
 
         const auto world = MWBase::Environment::get().getWorld();
-        const auto navigator = world->getNavigator();
 
         const int cellX = cell->getCell()->getGridX();
         const int cellY = cell->getCell()->getGridY();
@@ -441,12 +442,34 @@ namespace MWWorld
         if (!test && cell->getCell()->isExterior())
         {
             if (const auto heightField = mPhysics->getHeightField(cellX, cellY))
-                navigator->addObject(DetourNavigator::ObjectId(heightField), *heightField->getShape(),
+            {
-                        heightField->getCollisionObject()->getWorldTransform());
+                const osg::Vec2i cellPosition(cellX, cellY);
+                const btVector3& origin = heightField->getCollisionObject()->getWorldTransform().getOrigin();
+                const osg::Vec3f shift(origin.x(), origin.y(), origin.z());
+                const osg::ref_ptr<const ESMTerrain::LandObject> land = mRendering.getLandManager()->getLand(cellX, cellY);
+                const ESM::Land::LandData* const data = land == nullptr ? nullptr : land->getData(ESM::Land::DATA_VHGT);
+                const HeightfieldShape shape = [&] () -> HeightfieldShape
+                {
+                    if (data == nullptr)
+                    {
+                        return DetourNavigator::HeightfieldPlane {static_cast<float>(ESM::Land::DEFAULT_HEIGHT)};
+                    }
+                    else
+                    {
+                        DetourNavigator::HeightfieldSurface heights;
+                        heights.mHeights = data->mHeights;
+                        heights.mSize = static_cast<std::size_t>(ESM::Land::LAND_SIZE);
+                        heights.mMinHeight = data->mMinHeight;
+                        heights.mMaxHeight = data->mMaxHeight;
+                        return heights;
+                    }
+                } ();
+                mNavigator.addHeightfield(cellPosition, ESM::Land::REAL_SIZE, shift, shape);
+            }
         }
 
         if (const auto pathgrid = world->getStore().get<ESM::Pathgrid>().search(*cell->getCell()))
-            navigator->addPathgrid(*cell->getCell(), *pathgrid);
+            mNavigator.addPathgrid(*cell->getCell(), *pathgrid);
 
         // register local scripts
         // do this before insertCell, to make sure we don't add scripts from levelled creature spawning twice
@@ -472,13 +495,18 @@ namespace MWWorld
                 if (cell->getCell()->isExterior())
                 {
                     if (const auto heightField = mPhysics->getHeightField(cellX, cellY))
-                        navigator->addWater(osg::Vec2i(cellX, cellY), ESM::Land::REAL_SIZE,
+                    {
-                                cell->getWaterLevel(), heightField->getCollisionObject()->getWorldTransform());
+                        const btTransform& transform =heightField->getCollisionObject()->getWorldTransform();
+                        mNavigator.addWater(osg::Vec2i(cellX, cellY), ESM::Land::REAL_SIZE,
+                                            osg::Vec3f(static_cast<float>(transform.getOrigin().x()),
+                                                       static_cast<float>(transform.getOrigin().y()),
+                                                       waterLevel));
+                    }
                 }
                 else
                 {
-                    navigator->addWater(osg::Vec2i(cellX, cellY), std::numeric_limits<int>::max(),
+                    mNavigator.addWater(osg::Vec2i(cellX, cellY), std::numeric_limits<int>::max(),
-                            cell->getWaterLevel(), btTransform::getIdentity());
+                                        osg::Vec3f(0, 0, waterLevel));
                 }
             }
             else
@@ -486,7 +514,7 @@ namespace MWWorld
 
             const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr();
 
-            navigator->update(player.getRefData().getPosition().asVec3());
+            mNavigator.update(player.getRefData().getPosition().asVec3());
 
             if (!cell->isExterior() && !(cell->getCell()->mData.mFlags & ESM::Cell::QuasiEx))
                 mRendering.configureAmbient(cell->getCell());
@@ -569,9 +597,8 @@ namespace MWWorld
 
     void Scene::playerMoved(const osg::Vec3f &pos)
     {
-        const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
         const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr();
-        navigator->updatePlayerPosition(player.getRefData().getPosition().asVec3());
+        mNavigator.updatePlayerPosition(player.getRefData().getPosition().asVec3());
 
         if (!mCurrentCell || !mCurrentCell->isExterior())
             return;
@@ -996,9 +1023,8 @@ namespace MWWorld
             addObject(ptr, *mPhysics, mRendering, mPagedRefs, false);
             addObject(ptr, *mPhysics, mNavigator);
             MWBase::Environment::get().getWorld()->scaleObject(ptr, ptr.getCellRef().getScale());
-            const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
             const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr();
-            navigator->update(player.getRefData().getPosition().asVec3());
+            mNavigator.update(player.getRefData().getPosition().asVec3());
         }
         catch (std::exception& e)
         {
@@ -1011,16 +1037,15 @@ namespace MWWorld
         MWBase::Environment::get().getMechanicsManager()->remove (ptr);
         MWBase::Environment::get().getSoundManager()->stopSound3D (ptr);
         MWBase::Environment::get().getLuaManager()->objectRemovedFromScene(ptr);
-        const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
         if (const auto object = mPhysics->getObject(ptr))
         {
-            navigator->removeObject(DetourNavigator::ObjectId(object));
+            mNavigator.removeObject(DetourNavigator::ObjectId(object));
             const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr();
-            navigator->update(player.getRefData().getPosition().asVec3());
+            mNavigator.update(player.getRefData().getPosition().asVec3());
         }
         else if (mPhysics->getActor(ptr))
         {
-            navigator->removeAgent(MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(ptr));
+            mNavigator.removeAgent(MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(ptr));
         }
         mPhysics->remove(ptr);
         mRendering.removeObject (ptr);

apps/openmw_test_suite/detournavigator/navigator.cpp

@@ -4,6 +4,7 @@
 #include <components/detournavigator/exceptions.hpp>
 #include <components/misc/rng.hpp>
 #include <components/loadinglistener/loadinglistener.hpp>
+#include <components/esm/loadland.hpp>
 
 #include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
 #include <BulletCollision/CollisionShapes/btBoxShape.h>
@@ -38,6 +39,9 @@ namespace
         float mStepSize;
         AreaCosts mAreaCosts;
         Loading::Listener mListener;
+        const osg::Vec2i mCellPosition {0, 0};
+        const int mHeightfieldTileSize = ESM::Land::REAL_SIZE / (ESM::Land::LAND_SIZE - 1);
+        const osg::Vec3f mShift {0, 0, 0};
 
         DetourNavigatorNavigatorTest()
             : mPlayerPosition(0, 0, 0)
@@ -90,6 +94,19 @@ namespace
         return btHeightfieldTerrainShape(width, width, values.data(), heightScale, -greater, greater, upAxis, heightDataType, flipQuadEdges);
     }
 
+    template <std::size_t size>
+    HeightfieldSurface makeSquareHeightfieldSurface(const std::array<float, size>& values)
+    {
+        const auto [min, max] = std::minmax_element(values.begin(), values.end());
+        const float greater = std::max(std::abs(*min), std::abs(*max));
+        HeightfieldSurface surface;
+        surface.mHeights = values.data();
+        surface.mMinHeight = -greater;
+        surface.mMaxHeight = greater;
+        surface.mSize = static_cast<int>(std::sqrt(size));
+        return surface;
+    }
+
     TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty)
     {
         EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut),
@@ -115,18 +132,17 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, update_then_find_path_should_return_path)
     {
-        const std::array<btScalar, 5 * 5> heightfieldData {{
+        constexpr std::array<float, 5 * 5> heightfieldData {{
             0,   0,    0,    0,    0,
             0, -25,  -25,  -25,  -25,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
         }};
-        btHeightfieldTerrainShape shape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        shape.setLocalScaling(btVector3(128, 128, 1));
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent);
 
@@ -160,22 +176,21 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, add_object_should_change_navmesh)
     {
-        const std::array<btScalar, 5 * 5> heightfieldData {{
+        const std::array<float, 5 * 5> heightfieldData {{
             0,   0,    0,    0,    0,
             0, -25,  -25,  -25,  -25,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
         }};
-        btHeightfieldTerrainShape heightfieldShape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        heightfieldShape.setLocalScaling(btVector3(128, 128, 1));
 
         btBoxShape boxShape(btVector3(20, 20, 100));
         btCompoundShape compoundShape;
         compoundShape.addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(0, 0, 0)), &boxShape);
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addObject(ObjectId(&heightfieldShape), heightfieldShape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
@@ -243,22 +258,21 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, update_changed_object_should_change_navmesh)
     {
-        const std::array<btScalar, 5 * 5> heightfieldData {{
+        const std::array<float, 5 * 5> heightfieldData {{
             0,   0,    0,    0,    0,
             0, -25,  -25,  -25,  -25,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
         }};
-        btHeightfieldTerrainShape heightfieldShape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        heightfieldShape.setLocalScaling(btVector3(128, 128, 1));
 
         btBoxShape boxShape(btVector3(20, 20, 100));
         btCompoundShape compoundShape;
         compoundShape.addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(0, 0, 0)), &boxShape);
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addObject(ObjectId(&heightfieldShape), heightfieldShape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
         mNavigator->addObject(ObjectId(&compoundShape), compoundShape, btTransform::getIdentity());
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
@@ -327,16 +341,16 @@ namespace
         )) << mPath;
     }
 
-    TEST_F(DetourNavigatorNavigatorTest, for_overlapping_heightfields_should_use_higher)
+    TEST_F(DetourNavigatorNavigatorTest, for_overlapping_heightfields_objects_should_use_higher)
     {
-        const std::array<btScalar, 5 * 5> heightfieldData {{
+        const std::array<btScalar, 5 * 5> heightfieldData1 {{
             0,   0,    0,    0,    0,
             0, -25,  -25,  -25,  -25,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
         }};
-        btHeightfieldTerrainShape shape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        btHeightfieldTerrainShape shape = makeSquareHeightfieldTerrainShape(heightfieldData1);
         shape.setLocalScaling(btVector3(128, 128, 1));
 
         const std::array<btScalar, 5 * 5> heightfieldData2 {{
@@ -383,6 +397,31 @@ namespace
         )) << mPath;
     }
 
+    TEST_F(DetourNavigatorNavigatorTest, only_one_heightfield_per_cell_is_allowed)
+    {
+        const std::array<float, 5 * 5> heightfieldData1 {{
+            0,   0,    0,    0,    0,
+            0, -25,  -25,  -25,  -25,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+        }};
+        const HeightfieldSurface surface1 = makeSquareHeightfieldSurface(heightfieldData1);
+
+        const std::array<float, 5 * 5> heightfieldData2 {{
+            -25, -25, -25, -25, -25,
+            -25, -25, -25, -25, -25,
+            -25, -25, -25, -25, -25,
+            -25, -25, -25, -25, -25,
+            -25, -25, -25, -25, -25,
+        }};
+        const HeightfieldSurface surface2 = makeSquareHeightfieldSurface(heightfieldData2);
+
+        mNavigator->addAgent(mAgentHalfExtents);
+        EXPECT_TRUE(mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface1.mSize - 1), mShift, surface1));
+        EXPECT_FALSE(mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface2.mSize - 1), mShift, surface2));
+    }
+
     TEST_F(DetourNavigatorNavigatorTest, path_should_be_around_avoid_shape)
     {
         std::array<btScalar, 5 * 5> heightfieldData {{
@@ -441,19 +480,18 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_ground_lower_than_water_with_only_swim_flag)
     {
-        std::array<btScalar, 5 * 5> heightfieldData {{
+        std::array<float, 5 * 5> heightfieldData {{
             -50,  -50,  -50,  -50,    0,
             -50, -100, -150, -100,  -50,
             -50, -150, -200, -150, -100,
             -50, -100, -150, -100, -100,
               0,  -50, -100, -100, -100,
         }};
-        btHeightfieldTerrainShape shape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        shape.setLocalScaling(btVector3(128, 128, 1));
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, 300, btTransform::getIdentity());
+        mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, osg::Vec3f(0, 0, 300));
-        mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
@@ -486,7 +524,7 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_when_ground_cross_water_with_swim_and_walk_flags)
     {
-        std::array<btScalar, 7 * 7> heightfieldData {{
+        std::array<float, 7 * 7> heightfieldData {{
             0,    0,    0,    0,    0,    0, 0,
             0, -100, -100, -100, -100, -100, 0,
             0, -100, -150, -150, -150, -100, 0,
@@ -495,12 +533,11 @@ namespace
             0, -100, -100, -100, -100, -100, 0,
             0,    0,    0,    0,    0,    0, 0,
         }};
-        btHeightfieldTerrainShape shape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        shape.setLocalScaling(btVector3(128, 128, 1));
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, -25, btTransform::getIdentity());
+        mNavigator->addWater(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), osg::Vec3f(0, 0, -25));
-        mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
@@ -532,7 +569,7 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_when_ground_cross_water_with_max_int_cells_size_and_swim_and_walk_flags)
     {
-        std::array<btScalar, 7 * 7> heightfieldData {{
+        std::array<float, 7 * 7> heightfieldData {{
             0,    0,    0,    0,    0,    0, 0,
             0, -100, -100, -100, -100, -100, 0,
             0, -100, -150, -150, -150, -100, 0,
@@ -541,12 +578,11 @@ namespace
             0, -100, -100, -100, -100, -100, 0,
             0,    0,    0,    0,    0,    0, 0,
         }};
-        btHeightfieldTerrainShape shape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        shape.setLocalScaling(btVector3(128, 128, 1));
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
-        mNavigator->addWater(osg::Vec2i(0, 0), std::numeric_limits<int>::max(), -25, btTransform::getIdentity());
+        mNavigator->addWater(osg::Vec2i(0, 0), std::numeric_limits<int>::max(), osg::Vec3f(0, 0, -25));
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
@@ -578,7 +614,7 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_ground_when_ground_cross_water_with_only_walk_flag)
     {
-        std::array<btScalar, 7 * 7> heightfieldData {{
+        std::array<float, 7 * 7> heightfieldData {{
             0,    0,    0,    0,    0,    0, 0,
             0, -100, -100, -100, -100, -100, 0,
             0, -100, -150, -150, -150, -100, 0,
@@ -587,12 +623,11 @@ namespace
             0, -100, -100, -100, -100, -100, 0,
             0,    0,    0,    0,    0,    0, 0,
         }};
-        btHeightfieldTerrainShape shape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        shape.setLocalScaling(btVector3(128, 128, 1));
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, -25, btTransform::getIdentity());
+        mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, osg::Vec3f(0, 0, -25));
-        mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
@@ -622,7 +657,7 @@ namespace
         )) << mPath;
     }
 
-    TEST_F(DetourNavigatorNavigatorTest, update_remove_and_update_then_find_path_should_return_path)
+    TEST_F(DetourNavigatorNavigatorTest, update_object_remove_and_update_then_find_path_should_return_path)
     {
         const std::array<btScalar, 5 * 5> heightfieldData {{
             0,   0,    0,    0,    0,
@@ -675,20 +710,71 @@ namespace
         )) << mPath;
     }
 
-    TEST_F(DetourNavigatorNavigatorTest, update_then_find_random_point_around_circle_should_return_position)
+    TEST_F(DetourNavigatorNavigatorTest, update_heightfield_remove_and_update_then_find_path_should_return_path)
     {
-        const std::array<btScalar, 5 * 5> heightfieldData {{
+        const std::array<float, 5 * 5> heightfieldData {{
             0,   0,    0,    0,    0,
             0, -25,  -25,  -25,  -25,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
         }};
-        btHeightfieldTerrainShape shape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        shape.setLocalScaling(btVector3(128, 128, 1));
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
+        mNavigator->update(mPlayerPosition);
+        mNavigator->wait(mListener, WaitConditionType::allJobsDone);
+
+        mNavigator->removeHeightfield(mCellPosition);
+        mNavigator->update(mPlayerPosition);
+        mNavigator->wait(mListener, WaitConditionType::allJobsDone);
+
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
+        mNavigator->update(mPlayerPosition);
+        mNavigator->wait(mListener, WaitConditionType::allJobsDone);
+
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+
+        EXPECT_THAT(mPath, ElementsAre(
+            Vec3fEq(-204, 204, 1.99998295307159423828125),
+            Vec3fEq(-183.965301513671875, 183.965301513671875, 1.99998819828033447265625),
+            Vec3fEq(-163.9306182861328125, 163.9306182861328125, 1.99999344348907470703125),
+            Vec3fEq(-143.89593505859375, 143.89593505859375, -2.7206256389617919921875),
+            Vec3fEq(-123.86124420166015625, 123.86124420166015625, -13.1089839935302734375),
+            Vec3fEq(-103.8265533447265625, 103.8265533447265625, -23.4973468780517578125),
+            Vec3fEq(-83.7918548583984375, 83.7918548583984375, -33.885707855224609375),
+            Vec3fEq(-63.75716400146484375, 63.75716400146484375, -44.27407073974609375),
+            Vec3fEq(-43.72247314453125, 43.72247314453125, -54.662433624267578125),
+            Vec3fEq(-23.6877803802490234375, 23.6877803802490234375, -65.0507965087890625),
+            Vec3fEq(-3.653090000152587890625, 3.653090000152587890625, -75.43915557861328125),
+            Vec3fEq(16.3816013336181640625, -16.3816013336181640625, -69.749267578125),
+            Vec3fEq(36.416290283203125, -36.416290283203125, -60.4739532470703125),
+            Vec3fEq(56.450984954833984375, -56.450984954833984375, -51.1986236572265625),
+            Vec3fEq(76.4856719970703125, -76.4856719970703125, -41.92330169677734375),
+            Vec3fEq(96.52036285400390625, -96.52036285400390625, -31.46941375732421875),
+            Vec3fEq(116.5550537109375, -116.5550537109375, -19.597003936767578125),
+            Vec3fEq(136.5897369384765625, -136.5897369384765625, -7.724592685699462890625),
+            Vec3fEq(156.6244354248046875, -156.6244354248046875, 1.99999535083770751953125),
+            Vec3fEq(176.6591339111328125, -176.6591339111328125, 1.99999010562896728515625),
+            Vec3fEq(196.693817138671875, -196.693817138671875, 1.99998486042022705078125),
+            Vec3fEq(204, -204, 1.99998295307159423828125)
+        )) << mPath;
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, update_then_find_random_point_around_circle_should_return_position)
+    {
+        const std::array<float, 5 * 5> heightfieldData {{
+            0,   0,    0,    0,    0,
+            0, -25,  -25,  -25,  -25,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+        }};
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
+
+        mNavigator->addAgent(mAgentHalfExtents);
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
@@ -709,21 +795,20 @@ namespace
         mSettings.mAsyncNavMeshUpdaterThreads = 2;
         mNavigator.reset(new NavigatorImpl(mSettings));
 
-        const std::array<btScalar, 5 * 5> heightfieldData {{
+        const std::array<float, 5 * 5> heightfieldData {{
             0,   0,    0,    0,    0,
             0, -25,  -25,  -25,  -25,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
         }};
-        btHeightfieldTerrainShape heightfieldShape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        heightfieldShape.setLocalScaling(btVector3(128, 128, 1));
 
         const std::vector<btBoxShape> boxShapes(100, btVector3(20, 20, 100));
 
         mNavigator->addAgent(mAgentHalfExtents);
 
-        mNavigator->addObject(ObjectId(&heightfieldShape), heightfieldShape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
 
         for (std::size_t i = 0; i < boxShapes.size(); ++i)
         {
@@ -810,18 +895,17 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, update_then_raycast_should_return_position)
     {
-        const std::array<btScalar, 5 * 5> heightfieldData {{
+        const std::array<float, 5 * 5> heightfieldData {{
             0,   0,    0,    0,    0,
             0, -25,  -25,  -25,  -25,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
         }};
-        btHeightfieldTerrainShape shape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        shape.setLocalScaling(btVector3(128, 128, 1));
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
@@ -833,22 +917,21 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, update_for_oscillating_object_that_does_not_change_navmesh_should_not_trigger_navmesh_update)
     {
-        const std::array<btScalar, 5 * 5> heightfieldData {{
+        const std::array<float, 5 * 5> heightfieldData {{
             0,   0,    0,    0,    0,
             0, -25,  -25,  -25,  -25,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
             0, -25, -100, -100, -100,
         }};
-        btHeightfieldTerrainShape heightfieldShape = makeSquareHeightfieldTerrainShape(heightfieldData);
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
-        heightfieldShape.setLocalScaling(btVector3(128, 128, 1));
 
         const btBoxShape oscillatingBoxShape(btVector3(20, 20, 20));
         const btVector3 oscillatingBoxShapePosition(32, 32, 400);
         const btBoxShape boderBoxShape(btVector3(50, 50, 50));
 
         mNavigator->addAgent(mAgentHalfExtents);
-        mNavigator->addObject(ObjectId(&heightfieldShape), heightfieldShape, btTransform::getIdentity());
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
         mNavigator->addObject(ObjectId(&oscillatingBoxShape), oscillatingBoxShape,
                               btTransform(btMatrix3x3::getIdentity(), oscillatingBoxShapePosition));
         // add this box to make navmesh bound box independent from oscillatingBoxShape rotations
@@ -881,4 +964,41 @@ namespace
             ASSERT_EQ(navMesh->getNavMeshRevision(), 4);
         }
     }
+
+    TEST_F(DetourNavigatorNavigatorTest, should_provide_path_over_flat_heightfield)
+    {
+        const HeightfieldPlane plane {100};
+
+        mNavigator->addAgent(mAgentHalfExtents);
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * 4, mShift, plane);
+        mNavigator->update(mPlayerPosition);
+        mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent);
+
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+
+        EXPECT_THAT(mPath, ElementsAre(
+            Vec3fEq(-204, 204, 101.99999237060546875),
+            Vec3fEq(-183.965301513671875, 183.965301513671875, 101.99999237060546875),
+            Vec3fEq(-163.9306182861328125, 163.9306182861328125, 101.99999237060546875),
+            Vec3fEq(-143.89593505859375, 143.89593505859375, 101.99999237060546875),
+            Vec3fEq(-123.86124420166015625, 123.86124420166015625, 101.99999237060546875),
+            Vec3fEq(-103.8265533447265625, 103.8265533447265625, 101.99999237060546875),
+            Vec3fEq(-83.7918548583984375, 83.7918548583984375, 101.99999237060546875),
+            Vec3fEq(-63.75716400146484375, 63.75716400146484375, 101.99999237060546875),
+            Vec3fEq(-43.72247314453125, 43.72247314453125, 101.99999237060546875),
+            Vec3fEq(-23.6877803802490234375, 23.6877803802490234375, 101.99999237060546875),
+            Vec3fEq(-3.653090000152587890625, 3.653090000152587890625, 101.99999237060546875),
+            Vec3fEq(16.3816013336181640625, -16.3816013336181640625, 101.99999237060546875),
+            Vec3fEq(36.416290283203125, -36.416290283203125, 101.99999237060546875),
+            Vec3fEq(56.450984954833984375, -56.450984954833984375, 101.99999237060546875),
+            Vec3fEq(76.4856719970703125, -76.4856719970703125, 101.99999237060546875),
+            Vec3fEq(96.52036285400390625, -96.52036285400390625, 101.99999237060546875),
+            Vec3fEq(116.5550537109375, -116.5550537109375, 101.99999237060546875),
+            Vec3fEq(136.5897369384765625, -136.5897369384765625, 101.99999237060546875),
+            Vec3fEq(156.6244354248046875, -156.6244354248046875, 101.99999237060546875),
+            Vec3fEq(176.6591339111328125, -176.6591339111328125, 101.99999237060546875),
+            Vec3fEq(196.693817138671875, -196.693817138671875, 101.99999237060546875),
+            Vec3fEq(204, -204, 101.99999237060546875)
+        )) << mPath;
+    }
 }

apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp

@@ -7,9 +7,9 @@
 #include <components/detournavigator/ref.hpp>
 #include <components/detournavigator/preparednavmeshdatatuple.hpp>
 
-#include <RecastAlloc.h>
+#include <osg/Vec3f>
 
-#include <LinearMath/btTransform.h>
+#include <RecastAlloc.h>
 
 #include <gtest/gtest.h>
 
@@ -128,21 +128,29 @@ namespace
         return result;
     }
 
+    Mesh makeMesh()
+    {
+        std::vector<int> indices {{0, 1, 2}};
+        std::vector<float> vertices {{0, 0, 0, 1, 0, 0, 1, 1, 0}};
+        std::vector<AreaType> areaTypes {1, AreaType_ground};
+        return Mesh(std::move(indices), std::move(vertices), std::move(areaTypes));
+    }
+
     struct DetourNavigatorNavMeshTilesCacheTest : Test
     {
         const osg::Vec3f mAgentHalfExtents {1, 2, 3};
         const TilePosition mTilePosition {0, 0};
         const std::size_t mGeneration = 0;
         const std::size_t mRevision = 0;
-        const std::vector<int> mIndices {{0, 1, 2}};
+        const Mesh mMesh {makeMesh()};
-        const std::vector<float> mVertices {{0, 0, 0, 1, 0, 0, 1, 1, 0}};
+        const std::vector<Cell> mWater {};
-        const std::vector<AreaType> mAreaTypes {1, AreaType_ground};
+        const std::vector<Heightfield> mHeightfields {};
-        const std::vector<RecastMesh::Water> mWater {};
+        const std::vector<FlatHeightfield> mFlatHeightfields {};
-        const RecastMesh mRecastMesh {mGeneration, mRevision, mIndices, mVertices, mAreaTypes, mWater};
+        const RecastMesh mRecastMesh {mGeneration, mRevision, mMesh, mWater, mHeightfields, mFlatHeightfields};
         std::unique_ptr<PreparedNavMeshData> mPreparedNavMeshData {makePeparedNavMeshData(3)};
 
         const std::size_t mRecastMeshSize = sizeof(mRecastMesh) + getSize(mRecastMesh);
-        const std::size_t mRecastMeshWithWaterSize = mRecastMeshSize + sizeof(RecastMesh::Water);
+        const std::size_t mRecastMeshWithWaterSize = mRecastMeshSize + sizeof(Cell);
         const std::size_t mPreparedNavMeshDataSize = sizeof(*mPreparedNavMeshData) + getSize(*mPreparedNavMeshData);
     };
 
@@ -225,8 +233,8 @@ namespace
     {
         const std::size_t maxSize = 1;
         NavMeshTilesCache cache(maxSize);
-        const std::vector<RecastMesh::Water> water {1, RecastMesh::Water {1, btTransform::getIdentity()}};
+        const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh unexistentRecastMesh {mGeneration, mRevision, mIndices, mVertices, mAreaTypes, water};
+        const RecastMesh unexistentRecastMesh {mGeneration, mRevision, mMesh, water, mHeightfields, mFlatHeightfields};
 
         cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, std::move(mPreparedNavMeshData));
         EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, unexistentRecastMesh));
@@ -237,8 +245,8 @@ namespace
         const std::size_t maxSize = mRecastMeshWithWaterSize + mPreparedNavMeshDataSize;
         NavMeshTilesCache cache(maxSize);
 
-        const std::vector<RecastMesh::Water> water {1, RecastMesh::Water {1, btTransform::getIdentity()}};
+        const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mIndices, mVertices, mAreaTypes, water};
+        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water, mHeightfields, mFlatHeightfields};
         auto anotherPreparedNavMeshData = makePeparedNavMeshData(3);
         const auto copy = clone(*anotherPreparedNavMeshData);
 
@@ -255,8 +263,8 @@ namespace
         const std::size_t maxSize = mRecastMeshWithWaterSize + mPreparedNavMeshDataSize;
         NavMeshTilesCache cache(maxSize);
 
-        const std::vector<RecastMesh::Water> water {1, RecastMesh::Water {1, btTransform::getIdentity()}};
+        const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mIndices, mVertices, mAreaTypes, water};
+        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water, mHeightfields, mFlatHeightfields};
         auto anotherPreparedNavMeshData = makePeparedNavMeshData(3);
 
         const auto value = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh,
@@ -271,14 +279,14 @@ namespace
         NavMeshTilesCache cache(maxSize);
         const auto copy = clone(*mPreparedNavMeshData);
 
-        const std::vector<RecastMesh::Water> leastRecentlySetWater {1, RecastMesh::Water {1, btTransform::getIdentity()}};
+        const std::vector<Cell> leastRecentlySetWater {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh leastRecentlySetRecastMesh {mGeneration, mRevision, mIndices, mVertices,
+        const RecastMesh leastRecentlySetRecastMesh {mGeneration, mRevision, mMesh, leastRecentlySetWater,
-            mAreaTypes, leastRecentlySetWater};
+                    mHeightfields, mFlatHeightfields};
         auto leastRecentlySetData = makePeparedNavMeshData(3);
 
-        const std::vector<RecastMesh::Water> mostRecentlySetWater {1, RecastMesh::Water {2, btTransform::getIdentity()}};
+        const std::vector<Cell> mostRecentlySetWater {1, Cell {2, osg::Vec3f()}};
-        const RecastMesh mostRecentlySetRecastMesh {mGeneration, mRevision, mIndices, mVertices,
+        const RecastMesh mostRecentlySetRecastMesh {mGeneration, mRevision, mMesh, mostRecentlySetWater,
-            mAreaTypes, mostRecentlySetWater};
+                    mHeightfields, mFlatHeightfields};
         auto mostRecentlySetData = makePeparedNavMeshData(3);
 
         ASSERT_TRUE(cache.set(mAgentHalfExtents, mTilePosition, leastRecentlySetRecastMesh,
@@ -299,15 +307,15 @@ namespace
         const std::size_t maxSize = 2 * (mRecastMeshWithWaterSize + mPreparedNavMeshDataSize);
         NavMeshTilesCache cache(maxSize);
 
-        const std::vector<RecastMesh::Water> leastRecentlyUsedWater {1, RecastMesh::Water {1, btTransform::getIdentity()}};
+        const std::vector<Cell> leastRecentlyUsedWater {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh leastRecentlyUsedRecastMesh {mGeneration, mRevision, mIndices, mVertices,
+        const RecastMesh leastRecentlyUsedRecastMesh {mGeneration, mRevision, mMesh, leastRecentlyUsedWater,
-            mAreaTypes, leastRecentlyUsedWater};
+                    mHeightfields, mFlatHeightfields};
         auto leastRecentlyUsedData = makePeparedNavMeshData(3);
         const auto leastRecentlyUsedCopy = clone(*leastRecentlyUsedData);
 
-        const std::vector<RecastMesh::Water> mostRecentlyUsedWater {1, RecastMesh::Water {2, btTransform::getIdentity()}};
+        const std::vector<Cell> mostRecentlyUsedWater {1, Cell {2, osg::Vec3f()}};
-        const RecastMesh mostRecentlyUsedRecastMesh {mGeneration, mRevision, mIndices, mVertices,
+        const RecastMesh mostRecentlyUsedRecastMesh {mGeneration, mRevision, mMesh, mostRecentlyUsedWater,
-            mAreaTypes, mostRecentlyUsedWater};
+                    mHeightfields, mFlatHeightfields};
         auto mostRecentlyUsedData = makePeparedNavMeshData(3);
         const auto mostRecentlyUsedCopy = clone(*mostRecentlyUsedData);
 
@@ -340,8 +348,9 @@ namespace
         const std::size_t maxSize = 2 * (mRecastMeshWithWaterSize + mPreparedNavMeshDataSize);
         NavMeshTilesCache cache(maxSize);
 
-        const std::vector<RecastMesh::Water> water {1, RecastMesh::Water {1, btTransform::getIdentity()}};
+        const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh tooLargeRecastMesh {mGeneration, mRevision, mIndices, mVertices, mAreaTypes, water};
+        const RecastMesh tooLargeRecastMesh {mGeneration, mRevision, mMesh, water,
+                    mHeightfields, mFlatHeightfields};
         auto tooLargeData = makePeparedNavMeshData(10);
 
         cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, std::move(mPreparedNavMeshData));
@@ -354,13 +363,14 @@ namespace
         const std::size_t maxSize = 2 * (mRecastMeshWithWaterSize + mPreparedNavMeshDataSize);
         NavMeshTilesCache cache(maxSize);
 
-        const std::vector<RecastMesh::Water> anotherWater {1, RecastMesh::Water {1, btTransform::getIdentity()}};
+        const std::vector<Cell> anotherWater {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mIndices, mVertices, mAreaTypes, anotherWater};
+        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, anotherWater,
+                    mHeightfields, mFlatHeightfields};
         auto anotherData = makePeparedNavMeshData(3);
 
-        const std::vector<RecastMesh::Water> tooLargeWater {1, RecastMesh::Water {2, btTransform::getIdentity()}};
+        const std::vector<Cell> tooLargeWater {1, Cell {2, osg::Vec3f()}};
-        const RecastMesh tooLargeRecastMesh {mGeneration, mRevision, mIndices, mVertices,
+        const RecastMesh tooLargeRecastMesh {mGeneration, mRevision, mMesh, tooLargeWater,
-            mAreaTypes, tooLargeWater};
+                    mHeightfields, mFlatHeightfields};
         auto tooLargeData = makePeparedNavMeshData(10);
 
         const auto value = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh,
@@ -379,9 +389,8 @@ namespace
         const std::size_t maxSize = mRecastMeshWithWaterSize + mPreparedNavMeshDataSize;
         NavMeshTilesCache cache(maxSize);
 
-        const std::vector<RecastMesh::Water> water {1, RecastMesh::Water {1, btTransform::getIdentity()}};
+        const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mIndices, mVertices,
+        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water, mHeightfields, mFlatHeightfields};
-            mAreaTypes, water};
         auto anotherData = makePeparedNavMeshData(3);
 
         const auto firstCopy = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, std::move(mPreparedNavMeshData));
@@ -399,8 +408,8 @@ namespace
         const std::size_t maxSize = mRecastMeshWithWaterSize + mPreparedNavMeshDataSize;
         NavMeshTilesCache cache(maxSize);
 
-        const std::vector<RecastMesh::Water> water {1, RecastMesh::Water {1, btTransform::getIdentity()}};
+        const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mIndices, mVertices, mAreaTypes, water};
+        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water, mHeightfields, mFlatHeightfields};
         auto anotherData = makePeparedNavMeshData(3);
 
         cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, std::move(mPreparedNavMeshData));

apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp

@@ -1,15 +1,20 @@
 #include "operators.hpp"
 
 #include <components/detournavigator/recastmeshbuilder.hpp>
-#include <components/detournavigator/settings.hpp>
 #include <components/detournavigator/recastmesh.hpp>
 #include <components/detournavigator/exceptions.hpp>
+#include <components/esm/loadland.hpp>
+#include <components/misc/convert.hpp>
+#include <components/debug/debuglog.hpp>
 
 #include <BulletCollision/CollisionShapes/btBoxShape.h>
 #include <BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h>
 #include <BulletCollision/CollisionShapes/btTriangleMesh.h>
 #include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
 #include <BulletCollision/CollisionShapes/btCompoundShape.h>
+#include <BulletCollision/CollisionDispatch/btCollisionObject.h>
+
+#include <DetourCommon.h>
 
 #include <gtest/gtest.h>
 #include <gmock/gmock.h>
@@ -18,9 +23,38 @@
 
 namespace DetourNavigator
 {
-    static inline bool operator ==(const RecastMesh::Water& lhs, const RecastMesh::Water& rhs)
+    static inline bool operator ==(const Cell& lhs, const Cell& rhs)
     {
-        return lhs.mCellSize == rhs.mCellSize && lhs.mTransform == rhs.mTransform;
+        return lhs.mSize == rhs.mSize && lhs.mShift == rhs.mShift;
+    }
+
+    static inline bool operator==(const Heightfield& lhs, const Heightfield& rhs)
+    {
+        return makeTuple(lhs) == makeTuple(rhs);
+    }
+
+    static inline bool operator==(const FlatHeightfield& lhs, const FlatHeightfield& rhs)
+    {
+        return std::tie(lhs.mBounds, lhs.mHeight) == std::tie(rhs.mBounds, rhs.mHeight);
+    }
+
+    static inline std::ostream& operator<<(std::ostream& s, const FlatHeightfield& v)
+    {
+        return s << "FlatHeightfield {" << v.mBounds << ", " << v.mHeight << "}";
+    }
+
+    static inline std::ostream& operator<<(std::ostream& s, const Heightfield& v)
+    {
+        s << "Heightfield {.mBounds=" << v.mBounds
+          << ", .mLength=" << int(v.mLength)
+          << ", .mMinHeight=" << v.mMinHeight
+          << ", .mMaxHeight=" << v.mMaxHeight
+          << ", .mShift=" << v.mShift
+          << ", .mScale=" << v.mScale
+          << ", .mHeights={";
+        for (float h : v.mHeights)
+            s << h << ", ";
+        return s << "}}";
     }
 }
 
@@ -31,14 +65,12 @@ namespace
 
     struct DetourNavigatorRecastMeshBuilderTest : Test
     {
-        Settings mSettings;
         TileBounds mBounds;
         const std::size_t mGeneration = 0;
         const std::size_t mRevision = 0;
 
         DetourNavigatorRecastMeshBuilderTest()
         {
-            mSettings.mRecastScaleFactor = 1.0f;
             mBounds.mMin = osg::Vec2f(-std::numeric_limits<float>::max() * std::numeric_limits<float>::epsilon(),
                                       -std::numeric_limits<float>::max() * std::numeric_limits<float>::epsilon());
             mBounds.mMax = osg::Vec2f(std::numeric_limits<float>::max() * std::numeric_limits<float>::epsilon(),
@@ -48,11 +80,11 @@ namespace
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, create_for_empty_should_return_empty)
     {
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>());
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>());
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>());
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>());
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>());
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>());
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, add_bhv_triangle_mesh_shape)
@@ -61,16 +93,16 @@ namespace
         mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0));
         btBvhTriangleMeshShape shape(&mesh, true);
 
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(static_cast<const btCollisionShape&>(shape), btTransform::getIdentity(), AreaType_ground);
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            1, 0, -1,
+            -1, -1, 0,
-            -1, 0, 1,
+            -1, 1, 0,
-            -1, 0, -1,
+            1, -1, 0,
-        }));
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 1, 0}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, add_transformed_bhv_triangle_mesh_shape)
@@ -78,70 +110,70 @@ namespace
         btTriangleMesh mesh;
         mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0));
         btBvhTriangleMeshShape shape(&mesh, true);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape),
             btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)),
             AreaType_ground
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            2, 3, 0,
+            0, 0, 3,
-            0, 3, 4,
+            0, 4, 3,
-            0, 3, 0,
+            2, 0, 3,
-        }));
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 1, 0}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, add_heightfield_terrian_shape)
     {
         const std::array<btScalar, 4> heightfieldData {{0, 0, 0, 0}};
         btHeightfieldTerrainShape shape(2, 2, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(static_cast<const btCollisionShape&>(shape), btTransform::getIdentity(), AreaType_ground);
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            -0.5, 0, -0.5,
+            -0.5, -0.5, 0,
-            -0.5, 0, 0.5,
+            -0.5, 0.5, 0,
-            0.5, 0, -0.5,
+            0.5, -0.5, 0,
-            0.5, 0, 0.5,
+            0.5, 0.5, 0,
-        }));
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2, 2, 1, 3}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({0, 1, 2, 2, 1, 3}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground, AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground, AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, add_box_shape_should_produce_12_triangles)
     {
         btBoxShape shape(btVector3(1, 1, 2));
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(static_cast<const btCollisionShape&>(shape), btTransform::getIdentity(), AreaType_ground);
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            1, 2, 1,
+            -1, -1, -2,
-            -1, 2, 1,
+            -1, -1, 2,
-            1, 2, -1,
+            -1, 1, -2,
-            -1, 2, -1,
+            -1, 1, 2,
-            1, -2, 1,
+            1, -1, -2,
-            -1, -2, 1,
+            1, -1, 2,
-            1, -2, -1,
+            1, 1, -2,
-            -1, -2, -1,
+            1, 1, 2,
-        })) << recastMesh->getVertices();
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({
-            0, 2, 3,
-            3, 1, 0,
-            0, 4, 6,
-            6, 2, 0,
             0, 1, 5,
-            5, 4, 0,
+            0, 2, 3,
-            7, 5, 1,
+            0, 4, 6,
             1, 3, 7,
-            7, 3, 2,
             2, 6, 7,
-            7, 6, 4,
+            3, 1, 0,
             4, 5, 7,
-        })) << recastMesh->getIndices();
+            5, 4, 0,
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>(12, AreaType_ground));
+            6, 2, 0,
+            7, 3, 2,
+            7, 5, 1,
+            7, 6, 4,
+        })) << recastMesh->getMesh().getIndices();
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>(12, AreaType_ground));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, add_compound_shape)
@@ -157,44 +189,44 @@ namespace
         shape.addChildShape(btTransform::getIdentity(), &triangle1);
         shape.addChildShape(btTransform::getIdentity(), &box);
         shape.addChildShape(btTransform::getIdentity(), &triangle2);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape),
             btTransform::getIdentity(),
             AreaType_ground
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            -1, -2, -1,
+            -1, -1, -2,
-            -1, -2, 1,
+            -1, -1, 0,
-            -1, 0, -1,
+            -1, -1, 2,
-            -1, 0, 1,
+            -1, 1, -2,
-            -1, 2, -1,
+            -1, 1, 0,
-            -1, 2, 1,
+            -1, 1, 2,
-            1, -2, -1,
+            1, -1, -2,
-            1, -2, 1,
+            1, -1, 0,
-            1, 0, -1,
+            1, -1, 2,
-            1, 0, 1,
+            1, 1, -2,
-            1, 2, -1,
+            1, 1, 0,
-            1, 2, 1,
+            1, 1, 2,
-        })) << recastMesh->getVertices();
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({
-            8, 3, 2,
+            0, 2, 8,
-            11, 10, 4,
+            0, 3, 5,
-            4, 5, 11,
+            0, 6, 9,
-            11, 7, 6,
+            2, 5, 11,
-            6, 10, 11,
+            3, 9, 11,
-            11, 5, 1,
+            5, 2, 0,
-            1, 7, 11,
+            6, 8, 11,
-            0, 1, 5,
+            7, 4, 1,
-            5, 4, 0,
+            7, 4, 10,
-            0, 4, 10,
+            8, 6, 0,
-            10, 6, 0,
+            9, 3, 0,
-            0, 6, 7,
+            11, 5, 3,
-            7, 1, 0,
+            11, 8, 2,
-            8, 3, 9,
+            11, 9, 6,
-        })) << recastMesh->getIndices();
+        })) << recastMesh->getMesh().getIndices();
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>(14, AreaType_ground));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>(14, AreaType_ground));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, add_transformed_compound_shape)
@@ -204,20 +236,20 @@ namespace
         btBvhTriangleMeshShape triangle(&mesh, true);
         btCompoundShape shape;
         shape.addChildShape(btTransform::getIdentity(), &triangle);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape),
             btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)),
             AreaType_ground
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            2, 3, 0,
+            0, 0, 3,
-            0, 3, 4,
+            0, 4, 3,
-            0, 3, 0,
+            2, 0, 3,
-        }));
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 1, 0}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, add_transformed_compound_shape_with_transformed_bhv_triangle_shape)
@@ -228,20 +260,20 @@ namespace
         btCompoundShape shape;
         shape.addChildShape(btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)),
                             &triangle);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape),
             btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)),
             AreaType_ground
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            3, 12, 2,
+            1, 2, 12,
-            1, 12, 10,
+            1, 10, 12,
-            1, 12, 2,
+            3, 2, 12,
-        }));
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 1, 0}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, without_bounds_add_bhv_triangle_shape_should_not_filter_by_bounds)
@@ -250,48 +282,47 @@ namespace
         mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0));
         mesh.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0));
         btBvhTriangleMeshShape shape(&mesh, true);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape),
             btTransform::getIdentity(),
             AreaType_ground
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            1, 0, -1,
+            -3, -3, 0,
-            -1, 0, 1,
+            -3, -2, 0,
-            -1, 0, -1,
+            -2, -3, 0,
-            -2, 0, -3,
+            -1, -1, 0,
-            -3, 0, -2,
+            -1, 1, 0,
-            -3, 0, -3,
+            1, -1, 0,
-        }));
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2, 3, 4, 5}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 1, 0, 5, 4, 3}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>(2, AreaType_ground));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>(2, AreaType_ground));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_bhv_triangle_shape_should_filter_by_bounds)
     {
-        mSettings.mRecastScaleFactor = 0.1f;
+        mBounds.mMin = osg::Vec2f(-3, -3);
-        mBounds.mMin = osg::Vec2f(-3, -3) * mSettings.mRecastScaleFactor;
+        mBounds.mMax = osg::Vec2f(-2, -2);
-        mBounds.mMax = osg::Vec2f(-2, -2) * mSettings.mRecastScaleFactor;
         btTriangleMesh mesh;
         mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0));
         mesh.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0));
         btBvhTriangleMeshShape shape(&mesh, true);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape),
             btTransform::getIdentity(),
             AreaType_ground
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            -0.2f, 0, -0.3f,
+            -3, -3, 0,
-            -0.3f, 0, -0.2f,
+            -3, -2, 0,
-            -0.3f, 0, -0.3f,
+            -2, -3, 0,
-        }));
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 1, 0}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_rotated_by_x_bhv_triangle_shape_should_filter_by_bounds)
@@ -302,7 +333,7 @@ namespace
         mesh.addTriangle(btVector3(0, -1, -1), btVector3(0, -1, -1), btVector3(0, 1, -1));
         mesh.addTriangle(btVector3(0, -3, -3), btVector3(0, -3, -2), btVector3(0, -2, -3));
         btBvhTriangleMeshShape shape(&mesh, true);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape),
             btTransform(btQuaternion(btVector3(1, 0, 0),
@@ -310,13 +341,13 @@ namespace
             AreaType_ground
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_THAT(recastMesh->getVertices(), Pointwise(FloatNear(1e-5), std::vector<float>({
+        EXPECT_THAT(recastMesh->getMesh().getVertices(), Pointwise(FloatNear(1e-5), std::vector<float>({
-            0, -0.70710659027099609375, -3.535533905029296875,
+            0, -4.24264049530029296875, 4.44089209850062616169452667236328125e-16,
-            0, 0.707107067108154296875, -3.535533905029296875,
+            0, -3.535533905029296875, -0.707106769084930419921875,
-            0, 2.384185791015625e-07, -4.24264049530029296875,
+            0, -3.535533905029296875, 0.707106769084930419921875,
-        })));
+        }))) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({1, 2, 0}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_rotated_by_y_bhv_triangle_shape_should_filter_by_bounds)
@@ -327,7 +358,7 @@ namespace
         mesh.addTriangle(btVector3(-1, 0, -1), btVector3(-1, 0, 1), btVector3(1, 0, -1));
         mesh.addTriangle(btVector3(-3, 0, -3), btVector3(-3, 0, -2), btVector3(-2, 0, -3));
         btBvhTriangleMeshShape shape(&mesh, true);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape),
             btTransform(btQuaternion(btVector3(0, 1, 0),
@@ -335,13 +366,13 @@ namespace
             AreaType_ground
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_THAT(recastMesh->getVertices(), Pointwise(FloatNear(1e-5), std::vector<float>({
+        EXPECT_THAT(recastMesh->getMesh().getVertices(), Pointwise(FloatNear(1e-5), std::vector<float>({
-            -3.535533905029296875, -0.70710659027099609375, 0,
+            -4.24264049530029296875, 0, 4.44089209850062616169452667236328125e-16,
-            -3.535533905029296875, 0.707107067108154296875, 0,
+            -3.535533905029296875, 0, -0.707106769084930419921875,
-            -4.24264049530029296875, 2.384185791015625e-07, 0,
+            -3.535533905029296875, 0, 0.707106769084930419921875,
-        })));
+        }))) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({1, 2, 0}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_rotated_by_z_bhv_triangle_shape_should_filter_by_bounds)
@@ -352,7 +383,7 @@ namespace
         mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0));
         mesh.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0));
         btBvhTriangleMeshShape shape(&mesh, true);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape),
             btTransform(btQuaternion(btVector3(0, 0, 1),
@@ -360,13 +391,13 @@ namespace
             AreaType_ground
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_THAT(recastMesh->getVertices(), Pointwise(FloatNear(1e-5), std::vector<float>({
+        EXPECT_THAT(recastMesh->getMesh().getVertices(), Pointwise(FloatNear(1e-5), std::vector<float>({
-            1.41421353816986083984375, 0, 1.1920928955078125e-07,
+            -1.41421353816986083984375, -1.1102230246251565404236316680908203125e-16, 0,
-            -1.41421353816986083984375, 0, -1.1920928955078125e-07,
+            1.1102230246251565404236316680908203125e-16, -1.41421353816986083984375, 0,
-            1.1920928955078125e-07, 0, -1.41421353816986083984375,
+            1.41421353816986083984375, 1.1102230246251565404236316680908203125e-16, 0,
-        })));
+        }))) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 0, 1}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, flags_values_should_be_corresponding_to_added_objects)
@@ -377,7 +408,7 @@ namespace
         btTriangleMesh mesh2;
         mesh2.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0));
         btBvhTriangleMeshShape shape2(&mesh2, true);
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(
             static_cast<const btCollisionShape&>(shape1),
             btTransform::getIdentity(),
@@ -389,25 +420,25 @@ namespace
             AreaType_null
         );
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            1, 0, -1,
+            -3, -3, 0,
-            -1, 0, 1,
+            -3, -2, 0,
-            -1, 0, -1,
+            -2, -3, 0,
-            -2, 0, -3,
+            -1, -1, 0,
-            -3, 0, -2,
+            -1, 1, 0,
-            -3, 0, -3,
+            1, -1, 0,
-        }));
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2, 3, 4, 5}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 1, 0, 5, 4, 3}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground, AreaType_null}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_null, AreaType_ground}));
     }
 
     TEST_F(DetourNavigatorRecastMeshBuilderTest, add_water_then_get_water_should_return_it)
     {
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
-        builder.addWater(1000, btTransform(btMatrix3x3::getIdentity(), btVector3(100, 200, 300)));
+        builder.addWater(1000, osg::Vec3f(100, 200, 300));
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getWater(), std::vector<RecastMesh::Water>({
+        EXPECT_EQ(recastMesh->getWater(), std::vector<Cell>({
-            RecastMesh::Water {1000, btTransform(btMatrix3x3::getIdentity(), btVector3(100, 200, 300))}
+            Cell {1000, osg::Vec3f(100, 200, 300)}
         }));
     }
 
@@ -418,16 +449,77 @@ namespace
         mesh.addTriangle(btVector3(1, 1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0));
         btBvhTriangleMeshShape shape(&mesh, true);
 
-        RecastMeshBuilder builder(mSettings, mBounds);
+        RecastMeshBuilder builder(mBounds);
         builder.addObject(static_cast<const btCollisionShape&>(shape), btTransform::getIdentity(), AreaType_ground);
         const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
-        EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
+        EXPECT_EQ(recastMesh->getMesh().getVertices(), std::vector<float>({
-            -1, 0, -1,
+            -1, -1, 0,
-            -1, 0, 1,
+            -1, 1, 0,
-            1, 0, -1,
+            1, -1, 0,
-            1, 0, 1,
+            1, 1, 0,
-        })) << recastMesh->getVertices();
+        })) << recastMesh->getMesh().getVertices();
-        EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({2, 1, 0, 2, 1, 3}));
+        EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 1, 0, 2, 1, 3}));
-        EXPECT_EQ(recastMesh->getAreaTypes(), std::vector<AreaType>({AreaType_ground, AreaType_ground}));
+        EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector<AreaType>({AreaType_ground, AreaType_ground}));
+    }
+
+    TEST_F(DetourNavigatorRecastMeshBuilderTest, add_flat_heightfield_should_add_intersection)
+    {
+        mBounds.mMin = osg::Vec2f(0, 0);
+        RecastMeshBuilder builder(mBounds);
+        builder.addHeightfield(1000, osg::Vec3f(1, 2, 3), 10);
+        const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
+        EXPECT_EQ(recastMesh->getFlatHeightfields(), std::vector<FlatHeightfield>({
+            FlatHeightfield {TileBounds {osg::Vec2f(0, 0), osg::Vec2f(501, 502)}, 13},
+        }));
+    }
+
+    TEST_F(DetourNavigatorRecastMeshBuilderTest, add_heightfield_inside_tile)
+    {
+        constexpr std::array<float, 9> heights {{
+            0, 1, 2,
+            3, 4, 5,
+            6, 7, 8,
+        }};
+        RecastMeshBuilder builder(mBounds);
+        builder.addHeightfield(1000, osg::Vec3f(1, 2, 3), heights.data(), 3, 0, 8);
+        const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
+        Heightfield expected;
+        expected.mBounds = TileBounds {osg::Vec2f(-499, -498), osg::Vec2f(501, 502)};
+        expected.mLength = 3;
+        expected.mMinHeight = 0;
+        expected.mMaxHeight = 8;
+        expected.mShift = osg::Vec3f(-499, -498, 3);
+        expected.mScale = 500;
+        expected.mHeights = {
+            0, 1, 2,
+            3, 4, 5,
+            6, 7, 8,
+        };
+        EXPECT_EQ(recastMesh->getHeightfields(), std::vector<Heightfield>({expected}));
+    }
+
+    TEST_F(DetourNavigatorRecastMeshBuilderTest, add_heightfield_should_add_intersection)
+    {
+        constexpr std::array<float, 9> heights {{
+            0, 1, 2,
+            3, 4, 5,
+            6, 7, 8,
+        }};
+        mBounds.mMin = osg::Vec2f(250, 250);
+        RecastMeshBuilder builder(mBounds);
+        builder.addHeightfield(1000, osg::Vec3f(-1, -2, 3), heights.data(), 3, 0, 8);
+        const auto recastMesh = std::move(builder).create(mGeneration, mRevision);
+        Heightfield expected;
+        expected.mBounds = TileBounds {osg::Vec2f(250, 250), osg::Vec2f(499, 498)};
+        expected.mLength = 2;
+        expected.mMinHeight = 0;
+        expected.mMaxHeight = 8;
+        expected.mShift = osg::Vec3f(-1, -2, 3);
+        expected.mScale = 500;
+        expected.mHeights = {
+            4, 5,
+            7, 8,
+        };
+        EXPECT_EQ(recastMesh->getHeightfields(), std::vector<Heightfield>({expected}));
     }
 }

apps/openmw_test_suite/detournavigator/tilecachedrecastmeshmanager.cpp

@@ -254,7 +254,7 @@ namespace
         TileCachedRecastMeshManager manager(mSettings);
         const osg::Vec2i cellPosition(0, 0);
         const int cellSize = 8192;
-        EXPECT_TRUE(manager.addWater(cellPosition, cellSize, btTransform::getIdentity()));
+        EXPECT_TRUE(manager.addWater(cellPosition, cellSize, osg::Vec3f()));
     }
 
     TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, add_water_for_not_max_int_should_add_new_tiles)
@@ -262,7 +262,7 @@ namespace
         TileCachedRecastMeshManager manager(mSettings);
         const osg::Vec2i cellPosition(0, 0);
         const int cellSize = 8192;
-        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, btTransform::getIdentity()));
+        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, osg::Vec3f()));
         for (int x = -6; x < 6; ++x)
             for (int y = -6; y < 6; ++y)
                 ASSERT_TRUE(manager.hasTile(TilePosition(x, y)));
@@ -275,7 +275,7 @@ namespace
         ASSERT_TRUE(manager.addObject(ObjectId(&boxShape), boxShape, btTransform::getIdentity(), AreaType::AreaType_ground));
         const osg::Vec2i cellPosition(0, 0);
         const int cellSize = std::numeric_limits<int>::max();
-        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, btTransform::getIdentity()));
+        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, osg::Vec3f()));
         for (int x = -6; x < 6; ++x)
             for (int y = -6; y < 6; ++y)
                 ASSERT_EQ(manager.hasTile(TilePosition(x, y)), -1 <= x && x <= 0 && -1 <= y && y <= 0);
@@ -292,10 +292,10 @@ namespace
         TileCachedRecastMeshManager manager(mSettings);
         const osg::Vec2i cellPosition(0, 0);
         const int cellSize = 8192;
-        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, btTransform::getIdentity()));
+        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, osg::Vec3f()));
         const auto result = manager.removeWater(cellPosition);
         ASSERT_TRUE(result.has_value());
-        EXPECT_EQ(result->mCellSize, cellSize);
+        EXPECT_EQ(result->mSize, cellSize);
     }
 
     TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, remove_water_for_existing_cell_should_remove_empty_tiles)
@@ -303,7 +303,7 @@ namespace
         TileCachedRecastMeshManager manager(mSettings);
         const osg::Vec2i cellPosition(0, 0);
         const int cellSize = 8192;
-        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, btTransform::getIdentity()));
+        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, osg::Vec3f()));
         ASSERT_TRUE(manager.removeWater(cellPosition));
         for (int x = -6; x < 6; ++x)
             for (int y = -6; y < 6; ++y)
@@ -317,10 +317,24 @@ namespace
         ASSERT_TRUE(manager.addObject(ObjectId(&boxShape), boxShape, btTransform::getIdentity(), AreaType::AreaType_ground));
         const osg::Vec2i cellPosition(0, 0);
         const int cellSize = 8192;
-        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, btTransform::getIdentity()));
+        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, osg::Vec3f()));
         ASSERT_TRUE(manager.removeWater(cellPosition));
         for (int x = -6; x < 6; ++x)
             for (int y = -6; y < 6; ++y)
                 ASSERT_EQ(manager.hasTile(TilePosition(x, y)), -1 <= x && x <= 0 && -1 <= y && y <= 0);
     }
+
+    TEST_F(DetourNavigatorTileCachedRecastMeshManagerTest, remove_object_should_not_remove_tile_with_water)
+    {
+        TileCachedRecastMeshManager manager(mSettings);
+        const osg::Vec2i cellPosition(0, 0);
+        const int cellSize = 8192;
+        const btBoxShape boxShape(btVector3(20, 20, 100));
+        ASSERT_TRUE(manager.addObject(ObjectId(&boxShape), boxShape, btTransform::getIdentity(), AreaType::AreaType_ground));
+        ASSERT_TRUE(manager.addWater(cellPosition, cellSize, osg::Vec3f()));
+        ASSERT_TRUE(manager.removeObject(ObjectId(&boxShape)));
+        for (int x = -6; x < 6; ++x)
+            for (int y = -6; y < 6; ++y)
+                ASSERT_TRUE(manager.hasTile(TilePosition(x, y)));
+    }
 }

components/detournavigator/cachedrecastmeshmanager.cpp

@@ -34,15 +34,15 @@ namespace DetourNavigator
     }
 
     bool CachedRecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize,
-        const btTransform& transform)
+        const osg::Vec3f& shift)
     {
-        if (!mImpl.addWater(cellPosition, cellSize, transform))
+        if (!mImpl.addWater(cellPosition, cellSize, shift))
             return false;
         mCached.reset();
         return true;
     }
 
-    std::optional<RecastMeshManager::Water> CachedRecastMeshManager::removeWater(const osg::Vec2i& cellPosition)
+    std::optional<Cell> CachedRecastMeshManager::removeWater(const osg::Vec2i& cellPosition)
     {
         const auto water = mImpl.removeWater(cellPosition);
         if (water)
@@ -50,6 +50,23 @@ namespace DetourNavigator
         return water;
     }
 
+    bool CachedRecastMeshManager::addHeightfield(const osg::Vec2i& cellPosition, int cellSize,
+        const osg::Vec3f& shift, const HeightfieldShape& shape)
+    {
+        if (!mImpl.addHeightfield(cellPosition, cellSize, shift, shape))
+            return false;
+        mCached.reset();
+        return true;
+    }
+
+    std::optional<Cell> CachedRecastMeshManager::removeHeightfield(const osg::Vec2i& cellPosition)
+    {
+        const auto cell = mImpl.removeHeightfield(cellPosition);
+        if (cell)
+            mCached.reset();
+        return cell;
+    }
+
     std::shared_ptr<RecastMesh> CachedRecastMeshManager::getMesh()
     {
         if (!mCached)

components/detournavigator/cachedrecastmeshmanager.hpp

@@ -3,6 +3,7 @@
 
 #include "recastmeshmanager.hpp"
 #include "version.hpp"
+#include "heightfieldshape.hpp"
 
 namespace DetourNavigator
 {
@@ -16,12 +17,17 @@ namespace DetourNavigator
 
         bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType);
 
-        bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform);
-
-        std::optional<RecastMeshManager::Water> removeWater(const osg::Vec2i& cellPosition);
-
         std::optional<RemovedRecastMeshObject> removeObject(const ObjectId id);
 
+        bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const osg::Vec3f& shift);
+
+        std::optional<Cell> removeWater(const osg::Vec2i& cellPosition);
+
+        bool addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift,
+            const HeightfieldShape& shape);
+
+        std::optional<Cell> removeHeightfield(const osg::Vec2i& cellPosition);
+
         std::shared_ptr<RecastMesh> getMesh();
 
         bool isEmpty() const;

components/detournavigator/debug.cpp

@@ -18,7 +18,7 @@ namespace DetourNavigator
         file.exceptions(std::ios::failbit | std::ios::badbit);
         file.precision(std::numeric_limits<float>::max_exponent10);
         std::size_t count = 0;
-        for (auto v : recastMesh.getVertices())
+        for (float v : recastMesh.getMesh().getVertices())
         {
             if (count % 3 == 0)
             {
@@ -31,7 +31,7 @@ namespace DetourNavigator
         }
         file << '\n';
         count = 0;
-        for (auto v : recastMesh.getIndices())
+        for (int v : recastMesh.getMesh().getIndices())
         {
             if (count % 3 == 0)
             {

components/detournavigator/gettilespositions.hpp

@@ -50,10 +50,11 @@ namespace DetourNavigator
     }
 
     template <class Callback>
-    void getTilesPositions(const int cellSize, const btTransform& transform,
+    void getTilesPositions(const int cellSize, const osg::Vec3f& shift,
         const Settings& settings, Callback&& callback)
     {
         const auto halfCellSize = cellSize / 2;
+        const btTransform transform(btMatrix3x3::getIdentity(), Misc::Convert::toBullet(shift));
         auto aabbMin = transform(btVector3(-halfCellSize, -halfCellSize, 0));
         auto aabbMax = transform(btVector3(halfCellSize, halfCellSize, 0));
 

components/detournavigator/heightfieldshape.hpp

@@ -0,0 +1,25 @@
+#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_HEIGHFIELDSHAPE_H
+#define OPENMW_COMPONENTS_DETOURNAVIGATOR_HEIGHFIELDSHAPE_H
+
+#include <cstddef>
+#include <variant>
+
+namespace DetourNavigator
+{
+    struct HeightfieldPlane
+    {
+        float mHeight;
+    };
+
+    struct HeightfieldSurface
+    {
+        const float* mHeights;
+        std::size_t mSize;
+        float mMinHeight;
+        float mMaxHeight;
+    };
+
+    using HeightfieldShape = std::variant<HeightfieldPlane, HeightfieldSurface>;
+}
+
+#endif

components/detournavigator/makenavmesh.cpp

@@ -9,8 +9,10 @@
 #include "navmeshtilescache.hpp"
 #include "preparednavmeshdata.hpp"
 #include "navmeshdata.hpp"
+#include "recastmeshbuilder.hpp"
 
 #include <components/misc/convert.hpp>
+#include <components/bullethelpers/processtrianglecallback.hpp>
 
 #include <DetourNavMesh.h>
 #include <DetourNavMeshBuilder.h>
@@ -28,32 +30,35 @@ namespace
 {
     using namespace DetourNavigator;
 
-    struct WaterBounds
+    struct Rectangle
     {
-        osg::Vec3f mMin;
+        TileBounds mBounds;
-        osg::Vec3f mMax;
+        float mHeight;
     };
 
-    WaterBounds getWaterBounds(const RecastMesh::Water& water, const Settings& settings,
+    Rectangle getSwimRectangle(const Cell& water, const Settings& settings,
         const osg::Vec3f& agentHalfExtents)
     {
-        if (water.mCellSize == std::numeric_limits<int>::max())
+        if (water.mSize == std::numeric_limits<int>::max())
         {
-            const auto transform = getSwimLevelTransform(settings, water.mTransform, agentHalfExtents.z());
+            return Rectangle {
-            const auto min = toNavMeshCoordinates(settings, Misc::Convert::makeOsgVec3f(transform(btVector3(-1, -1, 0))));
+                TileBounds {
-            const auto max = toNavMeshCoordinates(settings, Misc::Convert::makeOsgVec3f(transform(btVector3(1, 1, 0))));
+                    osg::Vec2f(-std::numeric_limits<float>::max(), -std::numeric_limits<float>::max()),
-            return WaterBounds {
+                    osg::Vec2f(std::numeric_limits<float>::max(), std::numeric_limits<float>::max())
-                osg::Vec3f(-std::numeric_limits<float>::max(), min.y(), -std::numeric_limits<float>::max()),
+                },
-                osg::Vec3f(std::numeric_limits<float>::max(), max.y(), std::numeric_limits<float>::max())
+                toNavMeshCoordinates(settings, getSwimLevel(settings, water.mShift.z(), agentHalfExtents.z()))
             };
         }
         else
         {
-            const auto transform = getSwimLevelTransform(settings, water.mTransform, agentHalfExtents.z());
+            const osg::Vec2f shift(water.mShift.x(), water.mShift.y());
-            const auto halfCellSize = water.mCellSize / 2.0f;
+            const float halfCellSize = water.mSize / 2.0f;
-            return WaterBounds {
+            return Rectangle {
-                toNavMeshCoordinates(settings, Misc::Convert::makeOsgVec3f(transform(btVector3(-halfCellSize, -halfCellSize, 0)))),
+                TileBounds{
-                toNavMeshCoordinates(settings, Misc::Convert::makeOsgVec3f(transform(btVector3(halfCellSize, halfCellSize, 0))))
+                    toNavMeshCoordinates(settings, shift + osg::Vec2f(-halfCellSize, -halfCellSize)),
+                    toNavMeshCoordinates(settings, shift + osg::Vec2f(halfCellSize, halfCellSize))
+                },
+                toNavMeshCoordinates(settings, getSwimLevel(settings, water.mShift.z(), agentHalfExtents.z()))
             };
         }
     }
@@ -157,28 +162,34 @@ namespace
             throw NavigatorException("Failed to create heightfield for navmesh");
     }
 
-    bool rasterizeSolidObjectsTriangles(rcContext& context, const RecastMesh& recastMesh, const rcConfig& config,
+    bool rasterizeTriangles(rcContext& context, const Mesh& mesh, const Settings& settings, const rcConfig& config,
         rcHeightfield& solid)
     {
-        const osg::Vec2f tileBoundsMin(config.bmin[0], config.bmin[2]);
+        std::vector<unsigned char> areas(mesh.getAreaTypes().begin(), mesh.getAreaTypes().end());
-        const osg::Vec2f tileBoundsMax(config.bmax[0], config.bmax[2]);
+        std::vector<float> vertices = mesh.getVertices();
-        std::vector<unsigned char> areas(recastMesh.getAreaTypes().begin(), recastMesh.getAreaTypes().end());
+
+        for (std::size_t i = 0; i < vertices.size(); i += 3)
+        {
+            for (std::size_t j = 0; j < 3; ++j)
+                vertices[i + j] = toNavMeshCoordinates(settings, vertices[i + j]);
+            std::swap(vertices[i + 1], vertices[i + 2]);
+        }
 
         rcClearUnwalkableTriangles(
             &context,
             config.walkableSlopeAngle,
-            recastMesh.getVertices().data(),
+            vertices.data(),
-            static_cast<int>(recastMesh.getVerticesCount()),
+            static_cast<int>(mesh.getVerticesCount()),
-            recastMesh.getIndices().data(),
+            mesh.getIndices().data(),
             static_cast<int>(areas.size()),
             areas.data()
         );
 
         return rcRasterizeTriangles(
             &context,
-            recastMesh.getVertices().data(),
+            vertices.data(),
-            static_cast<int>(recastMesh.getVerticesCount()),
+            static_cast<int>(mesh.getVerticesCount()),
-            recastMesh.getIndices().data(),
+            mesh.getIndices().data(),
             areas.data(),
             static_cast<int>(areas.size()),
             solid,
@@ -186,70 +197,92 @@ namespace
         );
     }
 
-    void rasterizeWaterTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
+    bool rasterizeTriangles(rcContext& context, const Rectangle& rectangle, const rcConfig& config,
+        const unsigned char* areas, std::size_t areasSize, rcHeightfield& solid)
+    {
+        const osg::Vec2f tileBoundsMin(
+            std::clamp(rectangle.mBounds.mMin.x(), config.bmin[0], config.bmax[0]),
+            std::clamp(rectangle.mBounds.mMin.y(), config.bmin[2], config.bmax[2])
+        );
+        const osg::Vec2f tileBoundsMax(
+            std::clamp(rectangle.mBounds.mMax.x(), config.bmin[0], config.bmax[0]),
+            std::clamp(rectangle.mBounds.mMax.y(), config.bmin[2], config.bmax[2])
+        );
+
+        if (tileBoundsMax == tileBoundsMin)
+            return true;
+
+        const std::array vertices {
+            tileBoundsMin.x(), rectangle.mHeight, tileBoundsMin.y(),
+            tileBoundsMin.x(), rectangle.mHeight, tileBoundsMax.y(),
+            tileBoundsMax.x(), rectangle.mHeight, tileBoundsMax.y(),
+            tileBoundsMax.x(), rectangle.mHeight, tileBoundsMin.y(),
+        };
+
+        const std::array indices {
+            0, 1, 2,
+            0, 2, 3,
+        };
+
+        return rcRasterizeTriangles(
+            &context,
+            vertices.data(),
+            static_cast<int>(vertices.size() / 3),
+            indices.data(),
+            areas,
+            static_cast<int>(areasSize),
+            solid,
+            config.walkableClimb
+        );
+    }
+
+    bool rasterizeTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const std::vector<Cell>& cells,
         const Settings& settings, const rcConfig& config, rcHeightfield& solid)
     {
         const std::array<unsigned char, 2> areas {{AreaType_water, AreaType_water}};
-
+        for (const Cell& cell : cells)
-        for (const auto& water : recastMesh.getWater())
         {
-            const auto bounds = getWaterBounds(water, settings, agentHalfExtents);
+            const Rectangle rectangle = getSwimRectangle(cell, settings, agentHalfExtents);
-
+            if (!rasterizeTriangles(context, rectangle, config, areas.data(), areas.size(), solid))
-            const osg::Vec2f tileBoundsMin(
+                return false;
-                std::min(config.bmax[0], std::max(config.bmin[0], bounds.mMin.x())),
-                std::min(config.bmax[2], std::max(config.bmin[2], bounds.mMin.z()))
-            );
-            const osg::Vec2f tileBoundsMax(
-                std::min(config.bmax[0], std::max(config.bmin[0], bounds.mMax.x())),
-                std::min(config.bmax[2], std::max(config.bmin[2], bounds.mMax.z()))
-            );
-
-            if (tileBoundsMax == tileBoundsMin)
-                continue;
-
-            const std::array<osg::Vec3f, 4> vertices {{
-                osg::Vec3f(tileBoundsMin.x(), bounds.mMin.y(), tileBoundsMin.y()),
-                osg::Vec3f(tileBoundsMin.x(), bounds.mMin.y(), tileBoundsMax.y()),
-                osg::Vec3f(tileBoundsMax.x(), bounds.mMin.y(), tileBoundsMax.y()),
-                osg::Vec3f(tileBoundsMax.x(), bounds.mMin.y(), tileBoundsMin.y()),
-            }};
-
-            std::array<float, 4 * 3> convertedVertices;
-            auto convertedVerticesIt = convertedVertices.begin();
-
-            for (const auto& vertex : vertices)
-                convertedVerticesIt = std::copy(vertex.ptr(), vertex.ptr() + 3, convertedVerticesIt);
-
-            const std::array<int, 6> indices {{
-                0, 1, 2,
-                0, 2, 3,
-            }};
-
-            const auto trianglesRasterized = rcRasterizeTriangles(
-                &context,
-                convertedVertices.data(),
-                static_cast<int>(convertedVertices.size() / 3),
-                indices.data(),
-                areas.data(),
-                static_cast<int>(areas.size()),
-                solid,
-                config.walkableClimb
-            );
-
-            if (!trianglesRasterized)
-                throw NavigatorException("Failed to create rasterize water triangles for navmesh");
         }
+        return true;
+    }
+
+    bool rasterizeTriangles(rcContext& context, const std::vector<FlatHeightfield>& heightfields,
+        const Settings& settings, const rcConfig& config, rcHeightfield& solid)
+    {
+        for (const FlatHeightfield& heightfield : heightfields)
+        {
+            const std::array<unsigned char, 2> areas {{AreaType_ground, AreaType_ground}};
+            const Rectangle rectangle {heightfield.mBounds, toNavMeshCoordinates(settings, heightfield.mHeight)};
+            if (!rasterizeTriangles(context, rectangle, config, areas.data(), areas.size(), solid))
+                return false;
+        }
+        return true;
+    }
+
+    bool rasterizeTriangles(rcContext& context, const std::vector<Heightfield>& heightfields,
+        const Settings& settings, const rcConfig& config, rcHeightfield& solid)
+    {
+        using BulletHelpers::makeProcessTriangleCallback;
+
+        for (const Heightfield& heightfield : heightfields)
+        {
+            const Mesh mesh = makeMesh(heightfield);
+            if (!rasterizeTriangles(context, mesh, settings, config, solid))
+                return false;
+        }
+        return true;
     }
 
     bool rasterizeTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
         const rcConfig& config, const Settings& settings, rcHeightfield& solid)
     {
-        if (!rasterizeSolidObjectsTriangles(context, recastMesh, config, solid))
+        return rasterizeTriangles(context, recastMesh.getMesh(), settings, config, solid)
-            return false;
+            && rasterizeTriangles(context, agentHalfExtents, recastMesh.getWater(), settings, config, solid)
-
+            && rasterizeTriangles(context, recastMesh.getHeightfields(), settings, config, solid)
-        rasterizeWaterTriangles(context, agentHalfExtents, recastMesh, settings, config, solid);
+            && rasterizeTriangles(context, recastMesh.getFlatHeightfields(), settings, config, solid);
-
-        return true;
     }
 
     void buildCompactHeightfield(rcContext& context, const int walkableHeight, const int walkableClimb,
@@ -485,7 +518,6 @@ namespace DetourNavigator
             " changedTileDistance=" << getDistance(changedTile, playerTile);
 
         const auto params = *navMeshCacheItem->lockConst()->getImpl().getParams();
-        const osg::Vec3f origin(params.orig[0], params.orig[1], params.orig[2]);
 
         if (!recastMesh)
         {
@@ -494,12 +526,14 @@ namespace DetourNavigator
         }
 
         auto recastMeshBounds = recastMesh->getBounds();
+        recastMeshBounds.mMin = toNavMeshCoordinates(settings, recastMeshBounds.mMin);
+        recastMeshBounds.mMax = toNavMeshCoordinates(settings, recastMeshBounds.mMax);
 
         for (const auto& water : recastMesh->getWater())
         {
-            const auto waterBounds = getWaterBounds(water, settings, agentHalfExtents);
+            const float height = toNavMeshCoordinates(settings, getSwimLevel(settings, water.mShift.z(), agentHalfExtents.z()));
-            recastMeshBounds.mMin.y() = std::min(recastMeshBounds.mMin.y(), waterBounds.mMin.y());
+            recastMeshBounds.mMin.y() = std::min(recastMeshBounds.mMin.y(), height);
-            recastMeshBounds.mMax.y() = std::max(recastMeshBounds.mMax.y(), waterBounds.mMax.y());
+            recastMeshBounds.mMax.y() = std::max(recastMeshBounds.mMax.y(), height);
         }
 
         if (isEmpty(recastMeshBounds))

components/detournavigator/navigator.hpp

@@ -8,6 +8,9 @@
 #include "navmeshcacheitem.hpp"
 #include "recastmeshtiles.hpp"
 #include "waitconditiontype.hpp"
+#include "heightfieldshape.hpp"
+
+#include <variant>
 
 namespace ESM
 {
@@ -134,14 +137,12 @@ namespace DetourNavigator
          * @brief addWater is used to set water level at given world cell.
          * @param cellPosition allows to distinguish cells if there is many in current world.
          * @param cellSize set cell borders. std::numeric_limits<int>::max() disables cell borders.
-         * @param level set z coordinate of water surface at the scene.
+         * @param shift set global shift of cell center.
-         * @param transform set global shift of cell center.
          * @return true if there was no water at given cell if cellSize != std::numeric_limits<int>::max() or there is
          * at least single object is added to the scene, false if there is already water for given cell or there is no
          * any other objects.
          */
-        virtual bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btScalar level,
+        virtual bool addWater(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift) = 0;
-            const btTransform& transform) = 0;
 
         /**
          * @brief removeWater to make it no more available at the scene.
@@ -150,6 +151,11 @@ namespace DetourNavigator
          */
         virtual bool removeWater(const osg::Vec2i& cellPosition) = 0;
 
+        virtual bool addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift,
+            const HeightfieldShape& shape) = 0;
+
+        virtual bool removeHeightfield(const osg::Vec2i& cellPosition) = 0;
+
         virtual void addPathgrid(const ESM::Cell& cell, const ESM::Pathgrid& pathgrid) = 0;
 
         virtual void removePathgrid(const ESM::Pathgrid& pathgrid) = 0;

components/detournavigator/navigatorimpl.cpp

@@ -2,6 +2,7 @@
 #include "debug.hpp"
 #include "settingsutils.hpp"
 
+#include <components/debug/debuglog.hpp>
 #include <components/esm/loadpgrd.hpp>
 #include <components/misc/coordinateconverter.hpp>
 
@@ -102,11 +103,9 @@ namespace DetourNavigator
         return result;
     }
 
-    bool NavigatorImpl::addWater(const osg::Vec2i& cellPosition, const int cellSize, const btScalar level,
+    bool NavigatorImpl::addWater(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift)
-        const btTransform& transform)
     {
-        return mNavMeshManager.addWater(cellPosition, cellSize,
+        return mNavMeshManager.addWater(cellPosition, cellSize, shift);
-            btTransform(transform.getBasis(), btVector3(transform.getOrigin().x(), transform.getOrigin().y(), level)));
     }
 
     bool NavigatorImpl::removeWater(const osg::Vec2i& cellPosition)
@@ -114,6 +113,17 @@ namespace DetourNavigator
         return mNavMeshManager.removeWater(cellPosition);
     }
 
+    bool NavigatorImpl::addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift,
+        const HeightfieldShape& shape)
+    {
+        return mNavMeshManager.addHeightfield(cellPosition, cellSize, shift, shape);
+    }
+
+    bool NavigatorImpl::removeHeightfield(const osg::Vec2i& cellPosition)
+    {
+        return mNavMeshManager.removeHeightfield(cellPosition);
+    }
+
     void NavigatorImpl::addPathgrid(const ESM::Cell& cell, const ESM::Pathgrid& pathgrid)
     {
         Misc::CoordinateConverter converter(&cell);

components/detournavigator/navigatorimpl.hpp

@@ -35,11 +35,15 @@ namespace DetourNavigator
 
         bool removeObject(const ObjectId id) override;
 
-        bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btScalar level,
+        bool addWater(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift) override;
-            const btTransform& transform) override;
 
         bool removeWater(const osg::Vec2i& cellPosition) override;
 
+        bool addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift,
+            const HeightfieldShape& shape) override;
+
+        bool removeHeightfield(const osg::Vec2i& cellPosition) override;
+
         void addPathgrid(const ESM::Cell& cell, const ESM::Pathgrid& pathgrid) override;
 
         void removePathgrid(const ESM::Pathgrid& pathgrid) override;

components/detournavigator/navigatorstub.hpp

@@ -54,8 +54,7 @@ namespace DetourNavigator
             return false;
         }
 
-        bool addWater(const osg::Vec2i& /*cellPosition*/, const int /*cellSize*/, const btScalar /*level*/,
+        bool addWater(const osg::Vec2i& /*cellPosition*/, int /*cellSize*/, const osg::Vec3f& /*shift*/) override
-            const btTransform& /*transform*/) override
         {
             return false;
         }
@@ -65,6 +64,17 @@ namespace DetourNavigator
             return false;
         }
 
+        bool addHeightfield(const osg::Vec2i& /*cellPosition*/, int /*cellSize*/, const osg::Vec3f& /*shift*/,
+            const HeightfieldShape& /*height*/) override
+        {
+            return false;
+        }
+
+        bool removeHeightfield(const osg::Vec2i& /*cellPosition*/) override
+        {
+            return false;
+        }
+
         void addPathgrid(const ESM::Cell& /*cell*/, const ESM::Pathgrid& /*pathgrid*/) override {}
 
         void removePathgrid(const ESM::Pathgrid& /*pathgrid*/) override {}

components/detournavigator/navmeshmanager.cpp

@@ -72,11 +72,11 @@ namespace DetourNavigator
         return true;
     }
 
-    bool NavMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform)
+    bool NavMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, const osg::Vec3f& shift)
     {
-        if (!mRecastMeshManager.addWater(cellPosition, cellSize, transform))
+        if (!mRecastMeshManager.addWater(cellPosition, cellSize, shift))
             return false;
-        addChangedTiles(cellSize, transform, ChangeType::add);
+        addChangedTiles(cellSize, shift, ChangeType::add);
         return true;
     }
 
@@ -85,7 +85,25 @@ namespace DetourNavigator
         const auto water = mRecastMeshManager.removeWater(cellPosition);
         if (!water)
             return false;
-        addChangedTiles(water->mCellSize, water->mTransform, ChangeType::remove);
+        addChangedTiles(water->mSize, water->mShift, ChangeType::remove);
+        return true;
+    }
+
+    bool NavMeshManager::addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift,
+        const HeightfieldShape& shape)
+    {
+        if (!mRecastMeshManager.addHeightfield(cellPosition, cellSize, shift, shape))
+            return false;
+        addChangedTiles(cellSize, shift, ChangeType::add);
+        return true;
+    }
+
+    bool NavMeshManager::removeHeightfield(const osg::Vec2i& cellPosition)
+    {
+        const auto heightfield = mRecastMeshManager.removeHeightfield(cellPosition);
+        if (!heightfield)
+            return false;
+        addChangedTiles(heightfield->mSize, heightfield->mShift, ChangeType::remove);
         return true;
     }
 
@@ -231,13 +249,13 @@ namespace DetourNavigator
             [&] (const TilePosition& v) { addChangedTile(v, changeType); });
     }
 
-    void NavMeshManager::addChangedTiles(const int cellSize, const btTransform& transform,
+    void NavMeshManager::addChangedTiles(const int cellSize, const osg::Vec3f& shift,
             const ChangeType changeType)
     {
         if (cellSize == std::numeric_limits<int>::max())
             return;
 
-        getTilesPositions(cellSize, transform, mSettings,
+        getTilesPositions(cellSize, shift, mSettings,
             [&] (const TilePosition& v) { addChangedTile(v, changeType); });
     }
 

components/detournavigator/navmeshmanager.hpp

@@ -6,6 +6,7 @@
 #include "offmeshconnectionsmanager.hpp"
 #include "recastmeshtiles.hpp"
 #include "waitconditiontype.hpp"
+#include "heightfieldshape.hpp"
 
 #include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
 
@@ -33,10 +34,15 @@ namespace DetourNavigator
 
         void addAgent(const osg::Vec3f& agentHalfExtents);
 
-        bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform);
+        bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const osg::Vec3f& shift);
 
         bool removeWater(const osg::Vec2i& cellPosition);
 
+        bool addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift,
+            const HeightfieldShape& shape);
+
+        bool removeHeightfield(const osg::Vec2i& cellPosition);
+
         bool reset(const osg::Vec3f& agentHalfExtents);
 
         void addOffMeshConnection(const ObjectId id, const osg::Vec3f& start, const osg::Vec3f& end, const AreaType areaType);
@@ -68,7 +74,7 @@ namespace DetourNavigator
 
         void addChangedTiles(const btCollisionShape& shape, const btTransform& transform, const ChangeType changeType);
 
-        void addChangedTiles(const int cellSize, const btTransform& transform, const ChangeType changeType);
+        void addChangedTiles(const int cellSize, const osg::Vec3f& shift, const ChangeType changeType);
 
         void addChangedTile(const TilePosition& tilePosition, const ChangeType changeType);
 

components/detournavigator/navmeshtilescache.cpp

@@ -42,7 +42,8 @@ namespace DetourNavigator
         while (!mFreeItems.empty() && mUsedNavMeshDataSize + itemSize > mMaxNavMeshDataSize)
             removeLeastRecentlyUsed();
 
-        RecastMeshData key {recastMesh.getIndices(), recastMesh.getVertices(), recastMesh.getAreaTypes(), recastMesh.getWater()};
+        RecastMeshData key {recastMesh.getMesh(), recastMesh.getWater(),
+                    recastMesh.getHeightfields(), recastMesh.getFlatHeightfields()};
 
         const auto iterator = mFreeItems.emplace(mFreeItems.end(), agentHalfExtents, changedTile, std::move(key), itemSize);
         const auto emplaced = mValues.emplace(std::make_tuple(agentHalfExtents, changedTile, std::cref(iterator->mRecastMeshData)), iterator);

components/detournavigator/navmeshtilescache.hpp

@@ -22,28 +22,28 @@ namespace DetourNavigator
 {
     struct RecastMeshData
     {
-        std::vector<int> mIndices;
+        Mesh mMesh;
-        std::vector<float> mVertices;
+        std::vector<Cell> mWater;
-        std::vector<AreaType> mAreaTypes;
+        std::vector<Heightfield> mHeightfields;
-        std::vector<RecastMesh::Water> mWater;
+        std::vector<FlatHeightfield> mFlatHeightfields;
     };
 
     inline bool operator <(const RecastMeshData& lhs, const RecastMeshData& rhs)
     {
-        return std::tie(lhs.mIndices, lhs.mVertices, lhs.mAreaTypes, lhs.mWater)
+        return std::tie(lhs.mMesh, lhs.mWater, lhs.mHeightfields, lhs.mFlatHeightfields)
-                < std::tie(rhs.mIndices, rhs.mVertices, rhs.mAreaTypes, rhs.mWater);
+                < std::tie(rhs.mMesh, rhs.mWater, rhs.mHeightfields, rhs.mFlatHeightfields);
     }
 
     inline bool operator <(const RecastMeshData& lhs, const RecastMesh& rhs)
     {
-        return std::tie(lhs.mIndices, lhs.mVertices, lhs.mAreaTypes, lhs.mWater)
+        return std::tie(lhs.mMesh, lhs.mWater, lhs.mHeightfields, lhs.mFlatHeightfields)
-                < std::tie(rhs.getIndices(), rhs.getVertices(), rhs.getAreaTypes(), rhs.getWater());
+                < std::tie(rhs.getMesh(), rhs.getWater(), rhs.getHeightfields(), rhs.getFlatHeightfields());
     }
 
     inline bool operator <(const RecastMesh& lhs, const RecastMeshData& rhs)
     {
-        return std::tie(lhs.getIndices(), lhs.getVertices(), lhs.getAreaTypes(), lhs.getWater())
+        return std::tie(lhs.getMesh(), lhs.getWater(), lhs.getHeightfields(), lhs.getFlatHeightfields())
-                < std::tie(rhs.mIndices, rhs.mVertices, rhs.mAreaTypes, rhs.mWater);
+                < std::tie(rhs.mMesh, rhs.mWater, rhs.mHeightfields, rhs.mFlatHeightfields);
     }
 
     class NavMeshTilesCache

components/detournavigator/recastmesh.cpp

@@ -5,23 +5,53 @@
 
 namespace DetourNavigator
 {
-    RecastMesh::RecastMesh(std::size_t generation, std::size_t revision, std::vector<int> indices, std::vector<float> vertices,
+    Mesh::Mesh(std::vector<int>&& indices, std::vector<float>&& vertices, std::vector<AreaType>&& areaTypes)
-            std::vector<AreaType> areaTypes, std::vector<Water> water)
+    {
+        if (indices.size() / 3 != areaTypes.size())
+           throw InvalidArgument("Number of flags doesn't match number of triangles: triangles="
+               + std::to_string(indices.size() / 3) + ", areaTypes=" + std::to_string(areaTypes.size()));
+        indices.shrink_to_fit();
+        vertices.shrink_to_fit();
+        areaTypes.shrink_to_fit();
+        mIndices = std::move(indices);
+        mVertices = std::move(vertices);
+        mAreaTypes = std::move(areaTypes);
+    }
+
+    RecastMesh::RecastMesh(std::size_t generation, std::size_t revision, Mesh mesh, std::vector<Cell> water,
+        std::vector<Heightfield> heightfields, std::vector<FlatHeightfield> flatHeightfields)
         : mGeneration(generation)
         , mRevision(revision)
-        , mIndices(std::move(indices))
+        , mMesh(std::move(mesh))
-        , mVertices(std::move(vertices))
-        , mAreaTypes(std::move(areaTypes))
         , mWater(std::move(water))
+        , mHeightfields(std::move(heightfields))
+        , mFlatHeightfields(std::move(flatHeightfields))
     {
-        if (getTrianglesCount() != mAreaTypes.size())
+        if (mMesh.getVerticesCount() > 0)
-            throw InvalidArgument("Number of flags doesn't match number of triangles: triangles="
+            rcCalcBounds(mMesh.getVertices().data(), static_cast<int>(mMesh.getVerticesCount()),
-                + std::to_string(getTrianglesCount()) + ", areaTypes=" + std::to_string(mAreaTypes.size()));
+                         mBounds.mMin.ptr(), mBounds.mMax.ptr());
-        if (getVerticesCount())
-            rcCalcBounds(mVertices.data(), static_cast<int>(getVerticesCount()), mBounds.mMin.ptr(), mBounds.mMax.ptr());
-        mIndices.shrink_to_fit();
-        mVertices.shrink_to_fit();
-        mAreaTypes.shrink_to_fit();
         mWater.shrink_to_fit();
+        mHeightfields.shrink_to_fit();
+        for (Heightfield& v : mHeightfields)
+            v.mHeights.shrink_to_fit();
+        for (const Heightfield& v : mHeightfields)
+        {
+            const auto [min, max] = std::minmax_element(v.mHeights.begin(), v.mHeights.end());
+            mBounds.mMin.x() = std::min(mBounds.mMin.x(), v.mBounds.mMin.x());
+            mBounds.mMin.y() = std::min(mBounds.mMin.y(), v.mBounds.mMin.y());
+            mBounds.mMin.z() = std::min(mBounds.mMin.z(), *min);
+            mBounds.mMax.x() = std::max(mBounds.mMax.x(), v.mBounds.mMax.x());
+            mBounds.mMax.y() = std::max(mBounds.mMax.y(), v.mBounds.mMax.y());
+            mBounds.mMax.z() = std::max(mBounds.mMax.z(), *max);
+        }
+        for (const FlatHeightfield& v : mFlatHeightfields)
+        {
+            mBounds.mMin.x() = std::min(mBounds.mMin.x(), v.mBounds.mMin.x());
+            mBounds.mMin.y() = std::min(mBounds.mMin.y(), v.mBounds.mMin.y());
+            mBounds.mMin.z() = std::min(mBounds.mMin.z(), v.mHeight);
+            mBounds.mMax.x() = std::max(mBounds.mMax.x(), v.mBounds.mMax.x());
+            mBounds.mMax.y() = std::max(mBounds.mMax.y(), v.mBounds.mMax.y());
+            mBounds.mMax.z() = std::max(mBounds.mMax.z(), v.mHeight);
+        }
     }
 }

components/detournavigator/recastmesh.hpp

@@ -3,29 +3,93 @@
 
 #include "areatype.hpp"
 #include "bounds.hpp"
+#include "tilebounds.hpp"
 
 #include <components/bullethelpers/operators.hpp>
 
+#include <osg/Vec3f>
+
 #include <memory>
 #include <string>
 #include <vector>
 #include <tuple>
-
+#include <numeric>
-#include <LinearMath/btTransform.h>
 
 namespace DetourNavigator
 {
+    class Mesh
+    {
+    public:
+        Mesh(std::vector<int>&& indices, std::vector<float>&& vertices, std::vector<AreaType>&& areaTypes);
+
+        const std::vector<int>& getIndices() const noexcept { return mIndices; }
+        const std::vector<float>& getVertices() const noexcept { return mVertices; }
+        const std::vector<AreaType>& getAreaTypes() const noexcept { return mAreaTypes; }
+        std::size_t getVerticesCount() const noexcept { return mVertices.size() / 3; }
+        std::size_t getTrianglesCount() const noexcept { return mAreaTypes.size(); }
+
+    private:
+        std::vector<int> mIndices;
+        std::vector<float> mVertices;
+        std::vector<AreaType> mAreaTypes;
+
+        friend inline bool operator<(const Mesh& lhs, const Mesh& rhs) noexcept
+        {
+            return std::tie(lhs.mIndices, lhs.mVertices, lhs.mAreaTypes)
+                    < std::tie(rhs.mIndices, rhs.mVertices, rhs.mAreaTypes);
+        }
+
+        friend inline std::size_t getSize(const Mesh& value) noexcept
+        {
+            return value.mIndices.size() * sizeof(int)
+                + value.mVertices.size() * sizeof(float)
+                + value.mAreaTypes.size() * sizeof(AreaType);
+        }
+    };
+
+    struct Cell
+    {
+        int mSize;
+        osg::Vec3f mShift;
+    };
+
+    struct Heightfield
+    {
+        TileBounds mBounds;
+        std::uint8_t mLength;
+        float mMinHeight;
+        float mMaxHeight;
+        osg::Vec3f mShift;
+        float mScale;
+        std::vector<float> mHeights;
+    };
+
+    inline auto makeTuple(const Heightfield& v) noexcept
+    {
+        return std::tie(v.mBounds, v.mLength, v.mMinHeight, v.mMaxHeight, v.mShift, v.mScale, v.mHeights);
+    }
+
+    inline bool operator<(const Heightfield& lhs, const Heightfield& rhs) noexcept
+    {
+        return makeTuple(lhs) < makeTuple(rhs);
+    }
+
+    struct FlatHeightfield
+    {
+        TileBounds mBounds;
+        float mHeight;
+    };
+
+    inline bool operator<(const FlatHeightfield& lhs, const FlatHeightfield& rhs) noexcept
+    {
+        return std::tie(lhs.mBounds, lhs.mHeight) < std::tie(rhs.mBounds, rhs.mHeight);
+    }
+
     class RecastMesh
     {
     public:
-        struct Water
+        RecastMesh(std::size_t generation, std::size_t revision, Mesh mesh, std::vector<Cell> water,
-        {
+            std::vector<Heightfield> heightfields, std::vector<FlatHeightfield> flatHeightfields);
-            int mCellSize;
-            btTransform mTransform;
-        };
-
-        RecastMesh(std::size_t generation, std::size_t revision, std::vector<int> indices, std::vector<float> vertices,
-            std::vector<AreaType> areaTypes, std::vector<Water> water);
 
         std::size_t getGeneration() const
         {
@@ -37,34 +101,21 @@ namespace DetourNavigator
             return mRevision;
         }
 
-        const std::vector<int>& getIndices() const
+        const Mesh& getMesh() const noexcept { return mMesh; }
-        {
-            return mIndices;
-        }
 
-        const std::vector<float>& getVertices() const
+        const std::vector<Cell>& getWater() const
-        {
-            return mVertices;
-        }
-
-        const std::vector<AreaType>& getAreaTypes() const
-        {
-            return mAreaTypes;
-        }
-
-        const std::vector<Water>& getWater() const
         {
             return mWater;
         }
 
-        std::size_t getVerticesCount() const
+        const std::vector<Heightfield>& getHeightfields() const noexcept
         {
-            return mVertices.size() / 3;
+            return mHeightfields;
         }
 
-        std::size_t getTrianglesCount() const
+        const std::vector<FlatHeightfield>& getFlatHeightfields() const noexcept
         {
-            return mIndices.size() / 3;
+            return mFlatHeightfields;
         }
 
         const Bounds& getBounds() const
@@ -75,31 +126,30 @@ namespace DetourNavigator
     private:
         std::size_t mGeneration;
         std::size_t mRevision;
-        std::vector<int> mIndices;
+        Mesh mMesh;
-        std::vector<float> mVertices;
+        std::vector<Cell> mWater;
-        std::vector<AreaType> mAreaTypes;
+        std::vector<Heightfield> mHeightfields;
-        std::vector<Water> mWater;
+        std::vector<FlatHeightfield> mFlatHeightfields;
         Bounds mBounds;
 
-        friend inline std::size_t getSize(const RecastMesh& recastMesh) noexcept
+        friend inline bool operator <(const RecastMesh& lhs, const RecastMesh& rhs) noexcept
         {
-            const std::size_t indicesSize = recastMesh.mIndices.size() * sizeof(int);
+            return std::tie(lhs.mMesh, lhs.mWater) < std::tie(rhs.mMesh, rhs.mWater);
-            const std::size_t verticesSize = recastMesh.mVertices.size() * sizeof(float);
+        }
-            const std::size_t areaTypesSize = recastMesh.mAreaTypes.size() * sizeof(AreaType);
+
-            const std::size_t waterSize = recastMesh.mWater.size() * sizeof(RecastMesh::Water);
+        friend inline std::size_t getSize(const RecastMesh& value) noexcept
-            return indicesSize + verticesSize + areaTypesSize + waterSize;
+        {
+            return getSize(value.mMesh) + value.mWater.size() * sizeof(Cell)
+                + value.mHeightfields.size() * sizeof(Heightfield)
+                + std::accumulate(value.mHeightfields.begin(), value.mHeightfields.end(), std::size_t {0},
+                                  [] (std::size_t r, const Heightfield& v) { return r + v.mHeights.size() * sizeof(float); })
+                + value.mFlatHeightfields.size() * sizeof(FlatHeightfield);
         }
     };
 
-    inline bool operator<(const RecastMesh::Water& lhs, const RecastMesh::Water& rhs)
+    inline bool operator<(const Cell& lhs, const Cell& rhs) noexcept
     {
-        return std::tie(lhs.mCellSize, lhs.mTransform) < std::tie(rhs.mCellSize, rhs.mTransform);
+        return std::tie(lhs.mSize, lhs.mShift) < std::tie(rhs.mSize, rhs.mShift);
-    }
-
-    inline bool operator <(const RecastMesh& lhs, const RecastMesh& rhs)
-    {
-        return std::tie(lhs.getIndices(), lhs.getVertices(), lhs.getAreaTypes(), lhs.getWater())
-                < std::tie(rhs.getIndices(), rhs.getVertices(), rhs.getAreaTypes(), rhs.getWater());
     }
 }
 

components/detournavigator/recastmeshbuilder.cpp

@@ -1,12 +1,11 @@
 #include "recastmeshbuilder.hpp"
 #include "debug.hpp"
-#include "settings.hpp"
-#include "settingsutils.hpp"
 #include "exceptions.hpp"
 
 #include <components/bullethelpers/transformboundingbox.hpp>
 #include <components/bullethelpers/processtrianglecallback.hpp>
 #include <components/misc/convert.hpp>
+#include <components/debug/debuglog.hpp>
 
 #include <BulletCollision/CollisionShapes/btBoxShape.h>
 #include <BulletCollision/CollisionShapes/btCompoundShape.h>
@@ -17,8 +16,8 @@
 
 #include <algorithm>
 #include <cassert>
-#include <tuple>
 #include <array>
+#include <vector>
 
 namespace DetourNavigator
 {
@@ -26,48 +25,105 @@ namespace DetourNavigator
 
     namespace
     {
-        void optimizeRecastMesh(std::vector<int>& indices, std::vector<float>& vertices)
+        RecastMeshTriangle makeRecastMeshTriangle(const btVector3* vertices, const AreaType areaType)
         {
-            std::vector<std::tuple<float, float, float>> uniqueVertices;
+            RecastMeshTriangle result;
-            uniqueVertices.reserve(vertices.size() / 3);
+            result.mAreaType = areaType;
+            for (std::size_t i = 0; i < 3; ++i)
+                result.mVertices[i] = Misc::Convert::makeOsgVec3f(vertices[i]);
+            return result;
+        }
 
-            for (std::size_t i = 0, n = vertices.size() / 3; i < n; ++i)
+        TileBounds maxCellTileBounds(int size, const osg::Vec3f& shift)
-                uniqueVertices.emplace_back(vertices[i * 3], vertices[i * 3 + 1], vertices[i * 3 + 2]);
+        {
-
+            const float halfCellSize = size / 2;
-            std::sort(uniqueVertices.begin(), uniqueVertices.end());
+            return TileBounds {
-            const auto end = std::unique(uniqueVertices.begin(), uniqueVertices.end());
+                osg::Vec2f(shift.x() - halfCellSize, shift.y() - halfCellSize),
-            uniqueVertices.erase(end, uniqueVertices.end());
+                osg::Vec2f(shift.x() + halfCellSize, shift.y() + halfCellSize)
-
+            };
-            if (uniqueVertices.size() == vertices.size() / 3)
-                return;
-
-            for (std::size_t i = 0, n = indices.size(); i < n; ++i)
-            {
-                const auto index = indices[i];
-                const auto vertex = std::make_tuple(vertices[index * 3], vertices[index * 3 + 1], vertices[index * 3 + 2]);
-                const auto it = std::lower_bound(uniqueVertices.begin(), uniqueVertices.end(), vertex);
-                assert(it != uniqueVertices.end());
-                assert(*it == vertex);
-                indices[i] = std::distance(uniqueVertices.begin(), it);
-            }
-
-            vertices.resize(uniqueVertices.size() * 3);
-
-            for (std::size_t i = 0, n = uniqueVertices.size(); i < n; ++i)
-            {
-                vertices[i * 3] = std::get<0>(uniqueVertices[i]);
-                vertices[i * 3 + 1] = std::get<1>(uniqueVertices[i]);
-                vertices[i * 3 + 2] = std::get<2>(uniqueVertices[i]);
-            }
         }
     }
 
-    RecastMeshBuilder::RecastMeshBuilder(const Settings& settings, const TileBounds& bounds)
+    Mesh makeMesh(std::vector<RecastMeshTriangle>&& triangles, const osg::Vec3f& shift)
-        : mSettings(settings)
+    {
-        , mBounds(bounds)
+        std::vector<osg::Vec3f> uniqueVertices;
+        uniqueVertices.reserve(3 * triangles.size());
+
+        for (const RecastMeshTriangle& v : triangles)
+            for (const osg::Vec3f& v : v.mVertices)
+                uniqueVertices.push_back(v);
+
+        std::sort(uniqueVertices.begin(), uniqueVertices.end());
+        uniqueVertices.erase(std::unique(uniqueVertices.begin(), uniqueVertices.end()), uniqueVertices.end());
+
+        std::vector<int> indices;
+        indices.reserve(3 * triangles.size());
+        std::vector<AreaType> areaTypes;
+        areaTypes.reserve(triangles.size());
+
+        for (const RecastMeshTriangle& v : triangles)
+        {
+            areaTypes.push_back(v.mAreaType);
+
+            for (const osg::Vec3f& v : v.mVertices)
+            {
+                const auto it = std::lower_bound(uniqueVertices.begin(), uniqueVertices.end(), v);
+                assert(it != uniqueVertices.end());
+                assert(*it == v);
+                indices.push_back(static_cast<int>(it - uniqueVertices.begin()));
+            }
+        }
+
+        triangles.clear();
+
+        std::vector<float> vertices;
+        vertices.reserve(3 * uniqueVertices.size());
+
+        for (const osg::Vec3f& v : uniqueVertices)
+        {
+            vertices.push_back(v.x() + shift.x());
+            vertices.push_back(v.y() + shift.y());
+            vertices.push_back(v.z() + shift.z());
+        }
+
+        return Mesh(std::move(indices), std::move(vertices), std::move(areaTypes));
+    }
+
+    Mesh makeMesh(const Heightfield& heightfield)
+    {
+        using BulletHelpers::makeProcessTriangleCallback;
+        using Misc::Convert::toOsg;
+
+        constexpr int upAxis = 2;
+        constexpr bool flipQuadEdges = false;
+#if BT_BULLET_VERSION < 310
+        std::vector<btScalar> heights(heightfield.mHeights.begin(), heightfield.mHeights.end());
+        btHeightfieldTerrainShape shape(static_cast<int>(heightfield.mHeights.size() / heightfield.mLength),
+            static_cast<int>(heightfield.mLength), heights.data(), 1,
+            heightfield.mMinHeight, heightfield.mMaxHeight, upAxis, PHY_FLOAT, flipQuadEdges
+        );
+#else
+        btHeightfieldTerrainShape shape(static_cast<int>(heightfield.mHeights.size() / heightfield.mLength),
+            static_cast<int>(heightfield.mLength), heightfield.mHeights.data(),
+            heightfield.mMinHeight, heightfield.mMaxHeight, upAxis, flipQuadEdges);
+#endif
+        shape.setLocalScaling(btVector3(heightfield.mScale, heightfield.mScale, 1));
+        btVector3 aabbMin;
+        btVector3 aabbMax;
+        shape.getAabb(btTransform::getIdentity(), aabbMin, aabbMax);
+        std::vector<RecastMeshTriangle> triangles;
+        auto callback = makeProcessTriangleCallback([&] (btVector3* vertices, int, int)
+        {
+            triangles.emplace_back(makeRecastMeshTriangle(vertices, AreaType_ground));
+        });
+        shape.processAllTriangles(&callback, aabbMin, aabbMax);
+        const osg::Vec2f shift = (osg::Vec2f(aabbMax.x(), aabbMax.y()) - osg::Vec2f(aabbMin.x(), aabbMin.y())) * 0.5;
+        return makeMesh(std::move(triangles), heightfield.mShift + osg::Vec3f(shift.x(), shift.y(), 0));
+    }
+
+    RecastMeshBuilder::RecastMeshBuilder(const TileBounds& bounds) noexcept
+        : mBounds(bounds)
     {
-        mBounds.mMin /= mSettings.get().mRecastScaleFactor;
-        mBounds.mMax /= mSettings.get().mRecastScaleFactor;
     }
 
     void RecastMeshBuilder::addObject(const btCollisionShape& shape, const btTransform& transform,
@@ -96,37 +152,26 @@ namespace DetourNavigator
     void RecastMeshBuilder::addObject(const btConcaveShape& shape, const btTransform& transform,
                                       const AreaType areaType)
     {
-        return addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* triangle, int, int)
+        return addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* vertices, int, int)
         {
-            for (std::size_t i = 3; i > 0; --i)
+            RecastMeshTriangle triangle = makeRecastMeshTriangle(vertices, areaType);
-                addTriangleVertex(triangle[i - 1]);
+            std::reverse(triangle.mVertices.begin(), triangle.mVertices.end());
-            mAreaTypes.push_back(areaType);
+            mTriangles.emplace_back(triangle);
         }));
     }
 
     void RecastMeshBuilder::addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform,
                                       const AreaType areaType)
     {
-        return addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* triangle, int, int)
+        addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* vertices, int, int)
         {
-            for (std::size_t i = 0; i < 3; ++i)
+            mTriangles.emplace_back(makeRecastMeshTriangle(vertices, areaType));
-                addTriangleVertex(triangle[i]);
-            mAreaTypes.push_back(areaType);
         }));
     }
 
     void RecastMeshBuilder::addObject(const btBoxShape& shape, const btTransform& transform, const AreaType areaType)
     {
-        const auto indexOffset = static_cast<int>(mVertices.size() / 3);
+        constexpr std::array<int, 36> indices {{
-
-        for (int vertex = 0, count = shape.getNumVertices(); vertex < count; ++vertex)
-        {
-            btVector3 position;
-            shape.getVertex(vertex, position);
-            addVertex(transform(position));
-        }
-
-        const std::array<int, 36> indices {{
             0, 2, 3,
             3, 1, 0,
             0, 4, 6,
@@ -141,22 +186,72 @@ namespace DetourNavigator
             4, 5, 7,
         }};
 
-        std::transform(indices.begin(), indices.end(), std::back_inserter(mIndices),
+        for (std::size_t i = 0; i < indices.size(); i += 3)
-            [&] (int index) { return index + indexOffset; });
+        {
-
+            std::array<btVector3, 3> vertices;
-        std::generate_n(std::back_inserter(mAreaTypes), 12, [=] { return areaType; });
+            for (std::size_t j = 0; j < 3; ++j)
+            {
+                btVector3 position;
+                shape.getVertex(indices[i + j], position);
+                vertices[j] = transform(position);
+            }
+            mTriangles.emplace_back(makeRecastMeshTriangle(vertices.data(), areaType));
+        }
     }
 
-    void RecastMeshBuilder::addWater(const int cellSize, const btTransform& transform)
+    void RecastMeshBuilder::addWater(const int cellSize, const osg::Vec3f& shift)
     {
-        mWater.push_back(RecastMesh::Water {cellSize, transform});
+        mWater.push_back(Cell {cellSize, shift});
+    }
+
+    void RecastMeshBuilder::addHeightfield(int cellSize, const osg::Vec3f& shift, float height)
+    {
+        if (const auto intersection = getIntersection(mBounds, maxCellTileBounds(cellSize, shift)))
+            mFlatHeightfields.emplace_back(FlatHeightfield {*intersection, height + shift.z()});
+    }
+
+    void RecastMeshBuilder::addHeightfield(int cellSize, const osg::Vec3f& shift, const float* heights,
+        std::size_t size, float minHeight, float maxHeight)
+    {
+        const auto intersection = getIntersection(mBounds, maxCellTileBounds(cellSize, shift));
+        if (!intersection.has_value())
+            return;
+        const float stepSize = static_cast<float>(cellSize) / (size - 1);
+        const int halfCellSize = cellSize / 2;
+        const auto local = [&] (float v, float shift) { return (v - shift + halfCellSize) / stepSize; };
+        const auto index = [&] (float v, int add) { return std::clamp<int>(static_cast<int>(v) + add, 0, size); };
+        const std::size_t minX = index(std::round(local(intersection->mMin.x(), shift.x())), -1);
+        const std::size_t minY = index(std::round(local(intersection->mMin.y(), shift.y())), -1);
+        const std::size_t maxX = index(std::round(local(intersection->mMax.x(), shift.x())), 1);
+        const std::size_t maxY = index(std::round(local(intersection->mMax.y(), shift.y())), 1);
+        const std::size_t endX = std::min(maxX + 1, size);
+        const std::size_t endY = std::min(maxY + 1, size);
+        const std::size_t sliceSize = (endX - minX) * (endY - minY);
+        if (sliceSize == 0)
+            return;
+        std::vector<float> tileHeights;
+        tileHeights.reserve(sliceSize);
+        for (std::size_t y = minY; y < endY; ++y)
+            for (std::size_t x = minX; x < endX; ++x)
+                tileHeights.push_back(heights[x + y * size]);
+        Heightfield heightfield;
+        heightfield.mBounds = *intersection;
+        heightfield.mLength = static_cast<std::uint8_t>(endY - minY);
+        heightfield.mMinHeight = minHeight;
+        heightfield.mMaxHeight = maxHeight;
+        heightfield.mShift = shift + osg::Vec3f(minX, minY, 0) * stepSize - osg::Vec3f(halfCellSize, halfCellSize, 0);
+        heightfield.mScale = stepSize;
+        heightfield.mHeights = std::move(tileHeights);
+        mHeightfields.emplace_back(heightfield);
     }
 
     std::shared_ptr<RecastMesh> RecastMeshBuilder::create(std::size_t generation, std::size_t revision) &&
     {
-        optimizeRecastMesh(mIndices, mVertices);
+        std::sort(mTriangles.begin(), mTriangles.end());
         std::sort(mWater.begin(), mWater.end());
-        return std::make_shared<RecastMesh>(generation, revision, std::move(mIndices), std::move(mVertices), std::move(mAreaTypes), std::move(mWater));
+        Mesh mesh = makeMesh(std::move(mTriangles));
+        return std::make_shared<RecastMesh>(generation, revision, std::move(mesh), std::move(mWater),
+                                            std::move(mHeightfields), std::move(mFlatHeightfields));
     }
 
     void RecastMeshBuilder::addObject(const btConcaveShape& shape, const btTransform& transform,
@@ -218,18 +313,4 @@ namespace DetourNavigator
 
         shape.processAllTriangles(&wrapper, aabbMin, aabbMax);
     }
-
-    void RecastMeshBuilder::addTriangleVertex(const btVector3& worldPosition)
-    {
-        mIndices.push_back(static_cast<int>(mVertices.size() / 3));
-        addVertex(worldPosition);
-    }
-
-    void RecastMeshBuilder::addVertex(const btVector3& worldPosition)
-    {
-        const auto navMeshPosition = toNavMeshCoordinates(mSettings, Misc::Convert::makeOsgVec3f(worldPosition));
-        mVertices.push_back(navMeshPosition.x());
-        mVertices.push_back(navMeshPosition.y());
-        mVertices.push_back(navMeshPosition.z());
-    }
 }

components/detournavigator/recastmeshbuilder.hpp

@@ -4,8 +4,16 @@
 #include "recastmesh.hpp"
 #include "tilebounds.hpp"
 
+#include <osg/Vec3f>
+
 #include <LinearMath/btTransform.h>
 
+#include <array>
+#include <functional>
+#include <memory>
+#include <tuple>
+#include <vector>
+
 class btBoxShape;
 class btCollisionShape;
 class btCompoundShape;
@@ -15,12 +23,21 @@ class btTriangleCallback;
 
 namespace DetourNavigator
 {
-    struct Settings;
+    struct RecastMeshTriangle
+    {
+        AreaType mAreaType;
+        std::array<osg::Vec3f, 3> mVertices;
+
+        friend inline bool operator<(const RecastMeshTriangle& lhs, const RecastMeshTriangle& rhs)
+        {
+            return std::tie(lhs.mAreaType, lhs.mVertices) < std::tie(rhs.mAreaType, rhs.mVertices);
+        }
+    };
 
     class RecastMeshBuilder
     {
     public:
-        RecastMeshBuilder(const Settings& settings, const TileBounds& bounds);
+        explicit RecastMeshBuilder(const TileBounds& bounds) noexcept;
 
         void addObject(const btCollisionShape& shape, const btTransform& transform, const AreaType areaType);
 
@@ -32,26 +49,30 @@ namespace DetourNavigator
 
         void addObject(const btBoxShape& shape, const btTransform& transform, const AreaType areaType);
 
-        void addWater(const int mCellSize, const btTransform& transform);
+        void addWater(const int mCellSize, const osg::Vec3f& shift);
+
+        void addHeightfield(int cellSize, const osg::Vec3f& shift, float height);
+
+        void addHeightfield(int cellSize, const osg::Vec3f& shift, const float* heights, std::size_t size,
+            float minHeight, float maxHeight);
 
         std::shared_ptr<RecastMesh> create(std::size_t generation, std::size_t revision) &&;
 
     private:
-        std::reference_wrapper<const Settings> mSettings;
+        const TileBounds mBounds;
-        TileBounds mBounds;
+        std::vector<RecastMeshTriangle> mTriangles;
-        std::vector<int> mIndices;
+        std::vector<Cell> mWater;
-        std::vector<float> mVertices;
+        std::vector<Heightfield> mHeightfields;
-        std::vector<AreaType> mAreaTypes;
+        std::vector<FlatHeightfield> mFlatHeightfields;
-        std::vector<RecastMesh::Water> mWater;
 
         void addObject(const btConcaveShape& shape, const btTransform& transform, btTriangleCallback&& callback);
 
         void addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform, btTriangleCallback&& callback);
-
-        void addTriangleVertex(const btVector3& worldPosition);
-
-        void addVertex(const btVector3& worldPosition);
     };
+
+    Mesh makeMesh(std::vector<RecastMeshTriangle>&& triangles, const osg::Vec3f& shift = osg::Vec3f());
+
+    Mesh makeMesh(const Heightfield& heightfield);
 }
 
 #endif

components/detournavigator/recastmeshmanager.cpp

@@ -1,5 +1,30 @@
 #include "recastmeshmanager.hpp"
 #include "recastmeshbuilder.hpp"
+#include "settings.hpp"
+#include "heightfieldshape.hpp"
+
+#include <components/debug/debuglog.hpp>
+
+#include <utility>
+
+namespace
+{
+    struct AddHeightfield
+    {
+        const DetourNavigator::Cell& mCell;
+        DetourNavigator::RecastMeshBuilder& mBuilder;
+
+        void operator()(const DetourNavigator::HeightfieldSurface& v)
+        {
+            mBuilder.addHeightfield(mCell.mSize, mCell.mShift, v.mHeights, v.mSize, v.mMinHeight, v.mMaxHeight);
+        }
+
+        void operator()(DetourNavigator::HeightfieldPlane v)
+        {
+            mBuilder.addHeightfield(mCell.mSize, mCell.mShift, v.mHeight);
+        }
+    };
+}
 
 namespace DetourNavigator
 {
@@ -16,9 +41,8 @@ namespace DetourNavigator
         const auto object = mObjects.lower_bound(id);
         if (object != mObjects.end() && object->first == id)
             return false;
-        const auto iterator = mObjectsOrder.emplace(mObjectsOrder.end(),
+        mObjects.emplace_hint(object, id,
             OscillatingRecastMeshObject(RecastMeshObject(shape, transform, areaType), mRevision + 1));
-        mObjects.emplace_hint(object, id, iterator);
         ++mRevision;
         return true;
     }
@@ -30,7 +54,7 @@ namespace DetourNavigator
             return false;
         const std::size_t lastChangeRevision = mLastNavMeshReportedChange.has_value()
                 ? mLastNavMeshReportedChange->mRevision : mRevision;
-        if (!object->second->update(transform, areaType, lastChangeRevision, mTileBounds))
+        if (!object->second.update(transform, areaType, lastChangeRevision, mTileBounds))
             return false;
         ++mRevision;
         return true;
@@ -41,54 +65,72 @@ namespace DetourNavigator
         const auto object = mObjects.find(id);
         if (object == mObjects.end())
             return std::nullopt;
-        const RemovedRecastMeshObject result {object->second->getImpl().getShape(), object->second->getImpl().getTransform()};
+        const RemovedRecastMeshObject result {object->second.getImpl().getShape(), object->second.getImpl().getTransform()};
-        mObjectsOrder.erase(object->second);
         mObjects.erase(object);
         ++mRevision;
         return result;
     }
 
-    bool RecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize,
+    bool RecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, const osg::Vec3f& shift)
-        const btTransform& transform)
     {
-        const auto iterator = mWaterOrder.emplace(mWaterOrder.end(), Water {cellSize, transform});
+        if (!mWater.emplace(cellPosition, Cell {cellSize, shift}).second)
-        if (!mWater.emplace(cellPosition, iterator).second)
-        {
-            mWaterOrder.erase(iterator);
             return false;
-        }
         ++mRevision;
         return true;
     }
 
-    std::optional<RecastMeshManager::Water> RecastMeshManager::removeWater(const osg::Vec2i& cellPosition)
+    std::optional<Cell> RecastMeshManager::removeWater(const osg::Vec2i& cellPosition)
     {
         const auto water = mWater.find(cellPosition);
         if (water == mWater.end())
             return std::nullopt;
         ++mRevision;
-        const auto result = *water->second;
+        const Cell result = water->second;
-        mWaterOrder.erase(water->second);
         mWater.erase(water);
         return result;
     }
 
+    bool RecastMeshManager::addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift,
+        const HeightfieldShape& shape)
+    {
+        if (!mHeightfields.emplace(cellPosition, Heightfield {Cell {cellSize, shift}, shape}).second)
+            return false;
+        ++mRevision;
+        return true;
+    }
+
+    std::optional<Cell> RecastMeshManager::removeHeightfield(const osg::Vec2i& cellPosition)
+    {
+        const auto it = mHeightfields.find(cellPosition);
+        if (it == mHeightfields.end())
+            return std::nullopt;
+        ++mRevision;
+        const auto result = std::make_optional(it->second.mCell);
+        mHeightfields.erase(it);
+        return result;
+    }
+
     std::shared_ptr<RecastMesh> RecastMeshManager::getMesh()
     {
-        RecastMeshBuilder builder(mSettings, mTileBounds);
+        TileBounds tileBounds = mTileBounds;
-        for (const auto& v : mWaterOrder)
+        tileBounds.mMin /= mSettings.mRecastScaleFactor;
-            builder.addWater(v.mCellSize, v.mTransform);
+        tileBounds.mMax /= mSettings.mRecastScaleFactor;
-        for (const auto& object : mObjectsOrder)
+        RecastMeshBuilder builder(tileBounds);
+        for (const auto& [k, v] : mWater)
+            builder.addWater(v.mSize, v.mShift);
+        for (const auto& [k, object] : mObjects)
         {
             const RecastMeshObject& v = object.getImpl();
             builder.addObject(v.getShape(), v.getTransform(), v.getAreaType());
         }
+        for (const auto& [cellPosition, v] : mHeightfields)
+            std::visit(AddHeightfield {v.mCell, builder}, v.mShape);
         return std::move(builder).create(mGeneration, mRevision);
     }
 
     bool RecastMeshManager::isEmpty() const
     {
-        return mObjects.empty();
+        return mObjects.empty() && mWater.empty() && mHeightfields.empty();
     }
 
     void RecastMeshManager::reportNavMeshChange(const Version& recastMeshVersion, const Version& navMeshVersion)

components/detournavigator/recastmeshmanager.hpp

@@ -4,15 +4,18 @@
 #include "oscillatingrecastmeshobject.hpp"
 #include "objectid.hpp"
 #include "version.hpp"
+#include "recastmesh.hpp"
+#include "heightfieldshape.hpp"
 
 #include <LinearMath/btTransform.h>
 
 #include <osg/Vec2i>
 
-#include <list>
 #include <map>
 #include <optional>
 #include <memory>
+#include <variant>
+#include <tuple>
 
 class btCollisionShape;
 
@@ -30,12 +33,6 @@ namespace DetourNavigator
     class RecastMeshManager
     {
     public:
-        struct Water
-        {
-            int mCellSize = 0;
-            btTransform mTransform;
-        };
-
         RecastMeshManager(const Settings& settings, const TileBounds& bounds, std::size_t generation);
 
         bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform,
@@ -43,12 +40,17 @@ namespace DetourNavigator
 
         bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType);
 
-        bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform);
-
-        std::optional<Water> removeWater(const osg::Vec2i& cellPosition);
-
         std::optional<RemovedRecastMeshObject> removeObject(const ObjectId id);
 
+        bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const osg::Vec3f& shift);
+
+        std::optional<Cell> removeWater(const osg::Vec2i& cellPosition);
+
+        bool addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift,
+            const HeightfieldShape& shape);
+
+        std::optional<Cell> removeHeightfield(const osg::Vec2i& cellPosition);
+
         std::shared_ptr<RecastMesh> getMesh();
 
         bool isEmpty() const;
@@ -64,14 +66,19 @@ namespace DetourNavigator
             Version mNavMeshVersion;
         };
 
+        struct Heightfield
+        {
+            Cell mCell;
+            HeightfieldShape mShape;
+        };
+
         const Settings& mSettings;
         std::size_t mRevision = 0;
         std::size_t mGeneration;
         TileBounds mTileBounds;
-        std::list<OscillatingRecastMeshObject> mObjectsOrder;
+        std::map<ObjectId, OscillatingRecastMeshObject> mObjects;
-        std::map<ObjectId, std::list<OscillatingRecastMeshObject>::iterator> mObjects;
+        std::map<osg::Vec2i, Cell> mWater;
-        std::list<Water> mWaterOrder;
+        std::map<osg::Vec2i, Heightfield> mHeightfields;
-        std::map<osg::Vec2i, std::list<Water>::iterator> mWater;
         std::optional<Report> mLastNavMeshReportedChange;
         std::optional<Report> mLastNavMeshReport;
     };

components/detournavigator/settingsutils.hpp

@@ -37,6 +37,11 @@ namespace DetourNavigator
         return value * settings.mRecastScaleFactor;
     }
 
+    inline osg::Vec2f toNavMeshCoordinates(const Settings& settings, osg::Vec2f position)
+    {
+        return position * settings.mRecastScaleFactor;
+    }
+
     inline osg::Vec3f toNavMeshCoordinates(const Settings& settings, osg::Vec3f position)
     {
         std::swap(position.y(), position.z());
@@ -77,18 +82,9 @@ namespace DetourNavigator
         return static_cast<float>(settings.mBorderSize) * settings.mCellSize;
     }
 
-    inline float getSwimLevel(const Settings& settings, const float agentHalfExtentsZ)
+    inline float getSwimLevel(const Settings& settings, const float waterLevel, const float agentHalfExtentsZ)
     {
-        return - settings.mSwimHeightScale * agentHalfExtentsZ;
+        return waterLevel - settings.mSwimHeightScale * agentHalfExtentsZ - agentHalfExtentsZ;;
-    }
-
-    inline btTransform getSwimLevelTransform(const Settings& settings, const btTransform& transform,
-        const float agentHalfExtentsZ)
-    {
-        return btTransform(
-            transform.getBasis(),
-            transform.getOrigin() + btVector3(0, 0, getSwimLevel(settings, agentHalfExtentsZ) - agentHalfExtentsZ)
-        );
     }
 
     inline float getRealTileSize(const Settings& settings)

components/detournavigator/tilebounds.hpp

@@ -3,6 +3,10 @@
 
 #include <osg/Vec2f>
 
+#include <algorithm>
+#include <optional>
+#include <tuple>
+
 namespace DetourNavigator
 {
     struct TileBounds
@@ -10,6 +14,24 @@ namespace DetourNavigator
         osg::Vec2f mMin;
         osg::Vec2f mMax;
     };
+
+    inline bool operator<(const TileBounds& lhs, const TileBounds& rhs) noexcept
+    {
+        return std::tie(lhs.mMin, lhs.mMax) < std::tie(rhs.mMin, rhs.mMax);
+    }
+
+    inline std::optional<TileBounds> getIntersection(const TileBounds& a, const TileBounds& b) noexcept
+    {
+        const float minX = std::max(a.mMin.x(), b.mMin.x());
+        const float maxX = std::min(a.mMax.x(), b.mMax.x());
+        if (minX > maxX)
+            return std::nullopt;
+        const float minY = std::max(a.mMin.y(), b.mMin.y());
+        const float maxY = std::min(a.mMax.y(), b.mMax.y());
+        if (minY > maxY)
+            return std::nullopt;
+        return TileBounds {osg::Vec2f(minX, minY), osg::Vec2f(maxX, maxY)};
+    }
 }
 
 #endif

components/detournavigator/tilecachedrecastmeshmanager.cpp

@@ -3,6 +3,8 @@
 #include "gettilespositions.hpp"
 #include "settingsutils.hpp"
 
+#include <components/debug/debuglog.hpp>
+
 #include <algorithm>
 #include <vector>
 
@@ -54,7 +56,7 @@ namespace DetourNavigator
     }
 
     bool TileCachedRecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize,
-        const btTransform& transform)
+        const osg::Vec3f& shift)
     {
         const auto border = getBorderSize(mSettings);
 
@@ -67,7 +69,7 @@ namespace DetourNavigator
             const auto tiles = mTiles.lock();
             for (auto& tile : *tiles)
             {
-                if (tile.second.addWater(cellPosition, cellSize, transform))
+                if (tile.second.addWater(cellPosition, cellSize, shift))
                 {
                     tilesPositions.push_back(tile.first);
                     result = true;
@@ -76,7 +78,7 @@ namespace DetourNavigator
         }
         else
         {
-            getTilesPositions(cellSize, transform, mSettings, [&] (const TilePosition& tilePosition)
+            getTilesPositions(cellSize, shift, mSettings, [&] (const TilePosition& tilePosition)
                 {
                     const auto tiles = mTiles.lock();
                     auto tile = tiles->find(tilePosition);
@@ -88,7 +90,7 @@ namespace DetourNavigator
                         tile = tiles->insert(std::make_pair(tilePosition,
                                 CachedRecastMeshManager(mSettings, tileBounds, mTilesGeneration))).first;
                     }
-                    if (tile->second.addWater(cellPosition, cellSize, transform))
+                    if (tile->second.addWater(cellPosition, cellSize, shift))
                     {
                         tilesPositions.push_back(tilePosition);
                         result = true;
@@ -102,12 +104,12 @@ namespace DetourNavigator
         return result;
     }
 
-    std::optional<RecastMeshManager::Water> TileCachedRecastMeshManager::removeWater(const osg::Vec2i& cellPosition)
+    std::optional<Cell> TileCachedRecastMeshManager::removeWater(const osg::Vec2i& cellPosition)
     {
         const auto object = mWaterTilesPositions.find(cellPosition);
         if (object == mWaterTilesPositions.end())
             return std::nullopt;
-        std::optional<RecastMeshManager::Water> result;
+        std::optional<Cell> result;
         for (const auto& tilePosition : object->second)
         {
             const auto tiles = mTiles.lock();
@@ -128,6 +130,66 @@ namespace DetourNavigator
         return result;
     }
 
+    bool TileCachedRecastMeshManager::addHeightfield(const osg::Vec2i& cellPosition, int cellSize,
+        const osg::Vec3f& shift, const HeightfieldShape& shape)
+    {
+        const auto border = getBorderSize(mSettings);
+
+        auto& tilesPositions = mHeightfieldTilesPositions[cellPosition];
+
+        bool result = false;
+
+        getTilesPositions(cellSize, shift, mSettings, [&] (const TilePosition& tilePosition)
+            {
+                const auto tiles = mTiles.lock();
+                auto tile = tiles->find(tilePosition);
+                if (tile == tiles->end())
+                {
+                    auto tileBounds = makeTileBounds(mSettings, tilePosition);
+                    tileBounds.mMin -= osg::Vec2f(border, border);
+                    tileBounds.mMax += osg::Vec2f(border, border);
+                    tile = tiles->insert(std::make_pair(tilePosition,
+                            CachedRecastMeshManager(mSettings, tileBounds, mTilesGeneration))).first;
+                }
+                if (tile->second.addHeightfield(cellPosition, cellSize, shift, shape))
+                {
+                    tilesPositions.push_back(tilePosition);
+                    result = true;
+                }
+            });
+
+        if (result)
+            ++mRevision;
+
+        return result;
+    }
+
+    std::optional<Cell> TileCachedRecastMeshManager::removeHeightfield(const osg::Vec2i& cellPosition)
+    {
+        const auto object = mHeightfieldTilesPositions.find(cellPosition);
+        if (object == mHeightfieldTilesPositions.end())
+            return std::nullopt;
+        std::optional<Cell> result;
+        for (const auto& tilePosition : object->second)
+        {
+            const auto tiles = mTiles.lock();
+            const auto tile = tiles->find(tilePosition);
+            if (tile == tiles->end())
+                continue;
+            const auto tileResult = tile->second.removeHeightfield(cellPosition);
+            if (tile->second.isEmpty())
+            {
+                tiles->erase(tile);
+                ++mTilesGeneration;
+            }
+            if (tileResult && !result)
+                result = tileResult;
+        }
+        if (result)
+            ++mRevision;
+        return result;
+    }
+
     std::shared_ptr<RecastMesh> TileCachedRecastMeshManager::getMesh(const TilePosition& tilePosition)
     {
         const auto tiles = mTiles.lock();

components/detournavigator/tilecachedrecastmeshmanager.hpp

@@ -6,6 +6,7 @@
 #include "settingsutils.hpp"
 #include "gettilespositions.hpp"
 #include "version.hpp"
+#include "heightfieldshape.hpp"
 
 #include <components/misc/guarded.hpp>
 
@@ -76,9 +77,14 @@ namespace DetourNavigator
 
         std::optional<RemovedRecastMeshObject> removeObject(const ObjectId id);
 
-        bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform);
+        bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const osg::Vec3f& shift);
 
-        std::optional<RecastMeshManager::Water> removeWater(const osg::Vec2i& cellPosition);
+        std::optional<Cell> removeWater(const osg::Vec2i& cellPosition);
+
+        bool addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift,
+            const HeightfieldShape& shape);
+
+        std::optional<Cell> removeHeightfield(const osg::Vec2i& cellPosition);
 
         std::shared_ptr<RecastMesh> getMesh(const TilePosition& tilePosition);
 
@@ -100,6 +106,7 @@ namespace DetourNavigator
         Misc::ScopeGuarded<std::map<TilePosition, CachedRecastMeshManager>> mTiles;
         std::unordered_map<ObjectId, std::vector<TilePosition>> mObjectsTilesPositions;
         std::map<osg::Vec2i, std::vector<TilePosition>> mWaterTilesPositions;
+        std::map<osg::Vec2i, std::vector<TilePosition>> mHeightfieldTilesPositions;
         std::size_t mRevision = 0;
         std::size_t mTilesGeneration = 0;
 

components/sceneutil/recastmesh.cpp

@@ -3,11 +3,15 @@
 
 #include <components/detournavigator/settings.hpp>
 #include <components/detournavigator/recastmesh.hpp>
+#include <components/detournavigator/recastmeshbuilder.hpp>
 
 #include <RecastDebugDraw.h>
 
 #include <osg/Group>
 
+#include <algorithm>
+#include <vector>
+
 namespace
 {
     std::vector<float> calculateNormals(const std::vector<float>& vertices, const std::vector<int>& indices)
@@ -37,12 +41,30 @@ namespace SceneUtil
     osg::ref_ptr<osg::Group> createRecastMeshGroup(const DetourNavigator::RecastMesh& recastMesh,
         const DetourNavigator::Settings& settings)
     {
+        using namespace DetourNavigator;
+
         const osg::ref_ptr<osg::Group> group(new osg::Group);
-        DebugDraw debugDraw(*group, osg::Vec3f(0, 0, 0), 1.0f / settings.mRecastScaleFactor);
+        DebugDraw debugDraw(*group, osg::Vec3f(0, 0, 0), 1.0f);
-        const auto normals = calculateNormals(recastMesh.getVertices(), recastMesh.getIndices());
+        const DetourNavigator::Mesh& mesh = recastMesh.getMesh();
+        std::vector<int> indices = mesh.getIndices();
+        std::vector<float> vertices = mesh.getVertices();
+
+        for (const Heightfield& heightfield : recastMesh.getHeightfields())
+        {
+            const Mesh mesh = makeMesh(heightfield);
+            const int indexShift = static_cast<int>(vertices.size() / 3);
+            std::copy(mesh.getVertices().begin(), mesh.getVertices().end(), std::back_inserter(vertices));
+            std::transform(mesh.getIndices().begin(), mesh.getIndices().end(), std::back_inserter(indices),
+                           [&] (int index) { return index + indexShift; });
+        }
+
+        for (std::size_t i = 0; i < vertices.size(); i += 3)
+            std::swap(vertices[i + 1], vertices[i + 2]);
+
+        const auto normals = calculateNormals(vertices, indices);
         const auto texScale = 1.0f / (settings.mCellSize * 10.0f);
-        duDebugDrawTriMesh(&debugDraw, recastMesh.getVertices().data(), recastMesh.getVerticesCount(),
+        duDebugDrawTriMesh(&debugDraw, vertices.data(), static_cast<int>(vertices.size() / 3),
-            recastMesh.getIndices().data(), normals.data(), recastMesh.getTrianglesCount(), nullptr, texScale);
+            indices.data(), normals.data(), static_cast<int>(indices.size() / 3), nullptr, texScale);
         return group;
     }
 }