Store heightfields as array of heights instead of triangles

To reduce size of RecastMesh and therefore cache size.

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)
     {
@@ -94,12 +103,43 @@ namespace
         std::vector<float> vertices;
         std::vector<int> indices;
         std::vector<AreaType> areaTypes;
-        generateVertices(std::back_inserter(vertices), triangles * 1.946, random);
+        if (distribution(random) < 0.939)
-        generateIndices(std::back_inserter(indices), static_cast<int>(vertices.size() / 3) - 1, vertices.size() * 1.545, random);
+        {
-        generateAreaTypes(std::back_inserter(areaTypes), indices.size() / 3, random);
+            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)
     {
@@ -110,11 +150,12 @@ namespace
         Mesh mesh = generateMesh(triangles, random);
         std::vector<Cell> water;
         generateWater(std::back_inserter(water), 1, random);
-        RecastMesh recastMesh(generation, revision, std::move(mesh), std::move(water));
+        RecastMesh recastMesh(generation, revision, std::move(mesh), std::move(water),
+                              {generateHeightfield(random)}, {generateFlatHeightfield(random)});
         return Key {agentHalfExtents, tilePosition, std::move(recastMesh)};
     }
 
-    constexpr std::size_t trianglesPerTile = 438;
+    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"
@@ -422,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);
@@ -439,8 +442,30 @@ namespace MWWorld
         if (!test && cell->getCell()->isExterior())
         {
             if (const auto heightField = mPhysics->getHeightField(cellX, cellY))
-                mNavigator.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()))

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, 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, osg::Vec3f(0, 0, -25));
+        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,11 +578,10 @@ 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(), 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, 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_heightfield_remove_and_update_then_find_path_should_return_path)
+    {
+        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);
+
+        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<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);
 
@@ -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

@@ -144,7 +144,9 @@ namespace
         const std::size_t mRevision = 0;
         const Mesh mMesh {makeMesh()};
         const std::vector<Cell> mWater {};
-        const RecastMesh mRecastMesh {mGeneration, mRevision, mMesh, mWater};
+        const std::vector<Heightfield> mHeightfields {};
+        const std::vector<FlatHeightfield> mFlatHeightfields {};
+        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);
@@ -232,7 +234,7 @@ namespace
         const std::size_t maxSize = 1;
         NavMeshTilesCache cache(maxSize);
         const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh unexistentRecastMesh {mGeneration, mRevision, mMesh, 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));
@@ -244,7 +246,7 @@ namespace
         NavMeshTilesCache cache(maxSize);
 
         const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water};
+        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water, mHeightfields, mFlatHeightfields};
         auto anotherPreparedNavMeshData = makePeparedNavMeshData(3);
         const auto copy = clone(*anotherPreparedNavMeshData);
 
@@ -262,7 +264,7 @@ namespace
         NavMeshTilesCache cache(maxSize);
 
         const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water};
+        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water, mHeightfields, mFlatHeightfields};
         auto anotherPreparedNavMeshData = makePeparedNavMeshData(3);
 
         const auto value = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh,
@@ -278,11 +280,13 @@ namespace
         const auto copy = clone(*mPreparedNavMeshData);
 
         const std::vector<Cell> leastRecentlySetWater {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh leastRecentlySetRecastMesh {mGeneration, mRevision, mMesh, leastRecentlySetWater};
+        const RecastMesh leastRecentlySetRecastMesh {mGeneration, mRevision, mMesh, leastRecentlySetWater,
+                    mHeightfields, mFlatHeightfields};
         auto leastRecentlySetData = makePeparedNavMeshData(3);
 
         const std::vector<Cell> mostRecentlySetWater {1, Cell {2, osg::Vec3f()}};
-        const RecastMesh mostRecentlySetRecastMesh {mGeneration, mRevision, mMesh, mostRecentlySetWater};
+        const RecastMesh mostRecentlySetRecastMesh {mGeneration, mRevision, mMesh, mostRecentlySetWater,
+                    mHeightfields, mFlatHeightfields};
         auto mostRecentlySetData = makePeparedNavMeshData(3);
 
         ASSERT_TRUE(cache.set(mAgentHalfExtents, mTilePosition, leastRecentlySetRecastMesh,
@@ -304,12 +308,14 @@ namespace
         NavMeshTilesCache cache(maxSize);
 
         const std::vector<Cell> leastRecentlyUsedWater {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh leastRecentlyUsedRecastMesh {mGeneration, mRevision, mMesh, leastRecentlyUsedWater};
+        const RecastMesh leastRecentlyUsedRecastMesh {mGeneration, mRevision, mMesh, leastRecentlyUsedWater,
+                    mHeightfields, mFlatHeightfields};
         auto leastRecentlyUsedData = makePeparedNavMeshData(3);
         const auto leastRecentlyUsedCopy = clone(*leastRecentlyUsedData);
 
         const std::vector<Cell> mostRecentlyUsedWater {1, Cell {2, osg::Vec3f()}};
-        const RecastMesh mostRecentlyUsedRecastMesh {mGeneration, mRevision, mMesh, mostRecentlyUsedWater};
+        const RecastMesh mostRecentlyUsedRecastMesh {mGeneration, mRevision, mMesh, mostRecentlyUsedWater,
+                    mHeightfields, mFlatHeightfields};
         auto mostRecentlyUsedData = makePeparedNavMeshData(3);
         const auto mostRecentlyUsedCopy = clone(*mostRecentlyUsedData);
 
@@ -343,7 +349,8 @@ namespace
         NavMeshTilesCache cache(maxSize);
 
         const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh tooLargeRecastMesh {mGeneration, mRevision, mMesh, water};
+        const RecastMesh tooLargeRecastMesh {mGeneration, mRevision, mMesh, water,
+                    mHeightfields, mFlatHeightfields};
         auto tooLargeData = makePeparedNavMeshData(10);
 
         cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, std::move(mPreparedNavMeshData));
@@ -357,11 +364,13 @@ namespace
         NavMeshTilesCache cache(maxSize);
 
         const std::vector<Cell> anotherWater {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, anotherWater};
+        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, anotherWater,
+                    mHeightfields, mFlatHeightfields};
         auto anotherData = makePeparedNavMeshData(3);
 
         const std::vector<Cell> tooLargeWater {1, Cell {2, osg::Vec3f()}};
-        const RecastMesh tooLargeRecastMesh {mGeneration, mRevision, mMesh, tooLargeWater};
+        const RecastMesh tooLargeRecastMesh {mGeneration, mRevision, mMesh, tooLargeWater,
+                    mHeightfields, mFlatHeightfields};
         auto tooLargeData = makePeparedNavMeshData(10);
 
         const auto value = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh,
@@ -381,7 +390,7 @@ namespace
         NavMeshTilesCache cache(maxSize);
 
         const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water};
+        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, water, mHeightfields, mFlatHeightfields};
         auto anotherData = makePeparedNavMeshData(3);
 
         const auto firstCopy = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, std::move(mPreparedNavMeshData));
@@ -400,7 +409,7 @@ namespace
         NavMeshTilesCache cache(maxSize);
 
         const std::vector<Cell> water {1, Cell {1, osg::Vec3f()}};
-        const RecastMesh anotherRecastMesh {mGeneration, mRevision, mMesh, 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

@@ -3,12 +3,16 @@
 #include <components/detournavigator/recastmeshbuilder.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>
 
@@ -23,6 +27,35 @@ namespace DetourNavigator
     {
         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 << "}}";
+    }
 }
 
 namespace
@@ -428,4 +461,65 @@ namespace
         EXPECT_EQ(recastMesh->getMesh().getIndices(), std::vector<int>({2, 1, 0, 2, 1, 3}));
         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}));
+    }
 }

components/detournavigator/cachedrecastmeshmanager.cpp

@@ -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,11 +17,16 @@ namespace DetourNavigator
 
         bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType);
 
+        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);
 
-        std::optional<RemovedRecastMeshObject> removeObject(const ObjectId id);
+        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();
 

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>
@@ -195,64 +197,92 @@ namespace
         );
     }
 
-    void rasterizeWaterTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const std::vector<Cell>& cells,
+    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)
         {
-            const auto rectangle = getSwimRectangle(cell, 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], rectangle.mBounds.mMin.x())),
+        }
-                std::min(config.bmax[2], std::max(config.bmin[2], rectangle.mBounds.mMin.y()))
+        return true;
-            );
+    }
-            const osg::Vec2f tileBoundsMax(
-                std::min(config.bmax[0], std::max(config.bmin[0], rectangle.mBounds.mMax.x())),
-                std::min(config.bmax[2], std::max(config.bmin[2], rectangle.mBounds.mMax.y()))
-            );
-
-            if (tileBoundsMax == tileBoundsMin)
-                continue;
-
-            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 {
+    bool rasterizeTriangles(rcContext& context, const std::vector<FlatHeightfield>& heightfields,
-                0, 1, 2,
+        const Settings& settings, const rcConfig& config, rcHeightfield& solid)
-                0, 2, 3,
+    {
-            };
+        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;
+    }
 
-            const auto trianglesRasterized = rcRasterizeTriangles(
+    bool rasterizeTriangles(rcContext& context, const std::vector<Heightfield>& heightfields,
-                &context,
+        const Settings& settings, const rcConfig& config, rcHeightfield& solid)
-                vertices.data(),
+    {
-                static_cast<int>(vertices.size() / 3),
+        using BulletHelpers::makeProcessTriangleCallback;
-                indices.data(),
+
-                areas.data(),
+        for (const Heightfield& heightfield : heightfields)
-                static_cast<int>(areas.size()),
+        {
-                solid,
+            const Mesh mesh = makeMesh(heightfield);
-                config.walkableClimb
+            if (!rasterizeTriangles(context, mesh, settings, config, solid))
-            );
+                return false;
-
-            if (!trianglesRasterized)
-                throw NavigatorException("Failed to create rasterize water triangles for navmesh");
         }
+        return true;
     }
 
     bool rasterizeTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
         const rcConfig& config, const Settings& settings, rcHeightfield& solid)
     {
-        if (!rasterizeTriangles(context, recastMesh.getMesh(), settings, 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.getWater(), settings, config, solid);
+            && rasterizeTriangles(context, recastMesh.getFlatHeightfields(), settings, config, solid);
-
-        return true;
     }
 
     void buildCompactHeightfield(rcContext& context, const int walkableHeight, const int walkableClimb,

components/detournavigator/navigator.hpp

@@ -8,6 +8,9 @@
 #include "navmeshcacheitem.hpp"
 #include "recastmeshtiles.hpp"
 #include "waitconditiontype.hpp"
+#include "heightfieldshape.hpp"
+
+#include <variant>
 
 namespace ESM
 {
@@ -148,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>
 
@@ -112,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

@@ -39,6 +39,11 @@ namespace DetourNavigator
 
         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

@@ -64,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

@@ -89,6 +89,24 @@ namespace DetourNavigator
         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;
+    }
+
     void NavMeshManager::addAgent(const osg::Vec3f& agentHalfExtents)
     {
         auto cached = mCache.find(agentHalfExtents);

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>
 
@@ -37,6 +38,11 @@ namespace DetourNavigator
 
         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);

components/detournavigator/navmeshtilescache.cpp

@@ -42,7 +42,8 @@ namespace DetourNavigator
         while (!mFreeItems.empty() && mUsedNavMeshDataSize + itemSize > mMaxNavMeshDataSize)
             removeLeastRecentlyUsed();
 
-        RecastMeshData key {recastMesh.getMesh(), 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

@@ -24,24 +24,26 @@ namespace DetourNavigator
     {
         Mesh mMesh;
         std::vector<Cell> mWater;
+        std::vector<Heightfield> mHeightfields;
+        std::vector<FlatHeightfield> mFlatHeightfields;
     };
 
     inline bool operator <(const RecastMeshData& lhs, const RecastMeshData& rhs)
     {
-        return std::tie(lhs.mMesh, lhs.mWater)
+        return std::tie(lhs.mMesh, lhs.mWater, lhs.mHeightfields, lhs.mFlatHeightfields)
-                < std::tie(rhs.mMesh, 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.mMesh, lhs.mWater)
+        return std::tie(lhs.mMesh, lhs.mWater, lhs.mHeightfields, lhs.mFlatHeightfields)
-                < std::tie(rhs.getMesh(), 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.getMesh(), lhs.getWater())
+        return std::tie(lhs.getMesh(), lhs.getWater(), lhs.getHeightfields(), lhs.getFlatHeightfields())
-                < std::tie(rhs.mMesh, rhs.mWater);
+                < std::tie(rhs.mMesh, rhs.mWater, rhs.mHeightfields, rhs.mFlatHeightfields);
     }
 
     class NavMeshTilesCache

components/detournavigator/recastmesh.cpp

@@ -18,15 +18,40 @@ namespace DetourNavigator
         mAreaTypes = std::move(areaTypes);
     }
 
-    RecastMesh::RecastMesh(std::size_t generation, std::size_t revision, Mesh mesh, std::vector<Cell> water)
+    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)
         , mMesh(std::move(mesh))
         , mWater(std::move(water))
+        , mHeightfields(std::move(heightfields))
+        , mFlatHeightfields(std::move(flatHeightfields))
     {
         if (mMesh.getVerticesCount() > 0)
             rcCalcBounds(mMesh.getVertices().data(), static_cast<int>(mMesh.getVerticesCount()),
                          mBounds.mMin.ptr(), mBounds.mMax.ptr());
         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,6 +3,7 @@
 
 #include "areatype.hpp"
 #include "bounds.hpp"
+#include "tilebounds.hpp"
 
 #include <components/bullethelpers/operators.hpp>
 
@@ -12,6 +13,7 @@
 #include <string>
 #include <vector>
 #include <tuple>
+#include <numeric>
 
 namespace DetourNavigator
 {
@@ -51,10 +53,43 @@ namespace DetourNavigator
         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:
-        RecastMesh(std::size_t generation, std::size_t revision, Mesh mesh, std::vector<Cell> water);
+        RecastMesh(std::size_t generation, std::size_t revision, Mesh mesh, std::vector<Cell> water,
+            std::vector<Heightfield> heightfields, std::vector<FlatHeightfield> flatHeightfields);
 
         std::size_t getGeneration() const
         {
@@ -73,6 +108,16 @@ namespace DetourNavigator
             return mWater;
         }
 
+        const std::vector<Heightfield>& getHeightfields() const noexcept
+        {
+            return mHeightfields;
+        }
+
+        const std::vector<FlatHeightfield>& getFlatHeightfields() const noexcept
+        {
+            return mFlatHeightfields;
+        }
+
         const Bounds& getBounds() const
         {
             return mBounds;
@@ -83,6 +128,8 @@ namespace DetourNavigator
         std::size_t mRevision;
         Mesh mMesh;
         std::vector<Cell> mWater;
+        std::vector<Heightfield> mHeightfields;
+        std::vector<FlatHeightfield> mFlatHeightfields;
         Bounds mBounds;
 
         friend inline bool operator <(const RecastMesh& lhs, const RecastMesh& rhs) noexcept
@@ -92,7 +139,11 @@ namespace DetourNavigator
 
         friend inline std::size_t getSize(const RecastMesh& value) noexcept
         {
-            return getSize(value.mMesh) + value.mWater.size() * sizeof(Cell);
+            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);
         }
     };
 

components/detournavigator/recastmeshbuilder.cpp

@@ -33,9 +33,18 @@ namespace DetourNavigator
                 result.mVertices[i] = Misc::Convert::makeOsgVec3f(vertices[i]);
             return result;
         }
+
+        TileBounds maxCellTileBounds(int size, const osg::Vec3f& shift)
+        {
+            const float halfCellSize = size / 2;
+            return TileBounds {
+                osg::Vec2f(shift.x() - halfCellSize, shift.y() - halfCellSize),
+                osg::Vec2f(shift.x() + halfCellSize, shift.y() + halfCellSize)
+            };
+        }
     }
 
-    Mesh makeMesh(std::vector<RecastMeshTriangle>&& triangles)
+    Mesh makeMesh(std::vector<RecastMeshTriangle>&& triangles, const osg::Vec3f& shift)
     {
         std::vector<osg::Vec3f> uniqueVertices;
         uniqueVertices.reserve(3 * triangles.size());
@@ -72,14 +81,46 @@ namespace DetourNavigator
 
         for (const osg::Vec3f& v : uniqueVertices)
         {
-            vertices.push_back(v.x());
+            vertices.push_back(v.x() + shift.x());
-            vertices.push_back(v.y());
+            vertices.push_back(v.y() + shift.y());
-            vertices.push_back(v.z());
+            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)
     {
@@ -122,7 +163,7 @@ namespace DetourNavigator
     void RecastMeshBuilder::addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform,
                                       const AreaType areaType)
     {
-        return addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* vertices, int, int)
+        addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* vertices, int, int)
         {
             mTriangles.emplace_back(makeRecastMeshTriangle(vertices, areaType));
         }));
@@ -163,12 +204,54 @@ namespace DetourNavigator
         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) &&
     {
         std::sort(mTriangles.begin(), mTriangles.end());
         std::sort(mWater.begin(), mWater.end());
         Mesh mesh = makeMesh(std::move(mTriangles));
-        return std::make_shared<RecastMesh>(generation, revision, std::move(mesh), std::move(mWater));
+        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,

components/detournavigator/recastmeshbuilder.hpp

@@ -51,19 +51,28 @@ namespace DetourNavigator
 
         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:
         const TileBounds mBounds;
         std::vector<RecastMeshTriangle> mTriangles;
         std::vector<Cell> mWater;
+        std::vector<Heightfield> mHeightfields;
+        std::vector<FlatHeightfield> mFlatHeightfields;
 
         void addObject(const btConcaveShape& shape, const btTransform& transform, btTriangleCallback&& callback);
 
         void addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform, btTriangleCallback&& callback);
     };
 
-    Mesh makeMesh(std::vector<RecastMeshTriangle>&& triangles);
+    Mesh makeMesh(std::vector<RecastMeshTriangle>&& triangles, const osg::Vec3f& shift = osg::Vec3f());
+
+    Mesh makeMesh(const Heightfield& heightfield);
 }
 
 #endif

components/detournavigator/recastmeshmanager.cpp

@@ -1,6 +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
 {
@@ -66,6 +90,26 @@ namespace DetourNavigator
         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()
     {
         TileBounds tileBounds = mTileBounds;
@@ -79,12 +123,14 @@ namespace DetourNavigator
             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() && mWater.empty();
+        return mObjects.empty() && mWater.empty() && mHeightfields.empty();
     }
 
     void RecastMeshManager::reportNavMeshChange(const Version& recastMeshVersion, const Version& navMeshVersion)

components/detournavigator/recastmeshmanager.hpp

@@ -5,6 +5,7 @@
 #include "objectid.hpp"
 #include "version.hpp"
 #include "recastmesh.hpp"
+#include "heightfieldshape.hpp"
 
 #include <LinearMath/btTransform.h>
 
@@ -13,6 +14,8 @@
 #include <map>
 #include <optional>
 #include <memory>
+#include <variant>
+#include <tuple>
 
 class btCollisionShape;
 
@@ -37,11 +40,16 @@ namespace DetourNavigator
 
         bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType);
 
+        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);
 
-        std::optional<RemovedRecastMeshObject> removeObject(const ObjectId id);
+        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();
 
@@ -58,12 +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::map<ObjectId, OscillatingRecastMeshObject> mObjects;
         std::map<osg::Vec2i, Cell> mWater;
+        std::map<osg::Vec2i, Heightfield> mHeightfields;
         std::optional<Report> mLastNavMeshReportedChange;
         std::optional<Report> mLastNavMeshReport;
     };

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>
 
@@ -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>
 
@@ -80,6 +81,11 @@ namespace DetourNavigator
 
         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);
 
         bool hasTile(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,6 +3,7 @@
 
 #include <components/detournavigator/settings.hpp>
 #include <components/detournavigator/recastmesh.hpp>
+#include <components/detournavigator/recastmeshbuilder.hpp>
 
 #include <RecastDebugDraw.h>
 
@@ -45,15 +46,25 @@ namespace SceneUtil
         const osg::ref_ptr<osg::Group> group(new osg::Group);
         DebugDraw debugDraw(*group, osg::Vec3f(0, 0, 0), 1.0f);
         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, mesh.getIndices());
+        const auto normals = calculateNormals(vertices, indices);
         const auto texScale = 1.0f / (settings.mCellSize * 10.0f);
         duDebugDrawTriMesh(&debugDraw, vertices.data(), static_cast<int>(vertices.size() / 3),
-            mesh.getIndices().data(), normals.data(), static_cast<int>(mesh.getIndices().size() / 3), nullptr, texScale);
+            indices.data(), normals.data(), static_cast<int>(indices.size() / 3), nullptr, texScale);
         return group;
     }
 }