diff --git a/apps/benchmarks/detournavigator/navmeshtilescache.cpp b/apps/benchmarks/detournavigator/navmeshtilescache.cpp index e31514afaf..6c530db41c 100644 --- a/apps/benchmarks/detournavigator/navmeshtilescache.cpp +++ b/apps/benchmarks/detournavigator/navmeshtilescache.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include @@ -30,6 +31,14 @@ namespace return TilePosition(distribution(random), distribution(random)); } + template + TileBounds generateTileBounds(Random& random) + { + std::uniform_real_distribution distribution(0.0, 1.0); + const osg::Vec2f min(distribution(random), distribution(random)); + return TileBounds {min, min + osg::Vec2f(1.0, 1.0)}; + } + template osg::Vec3f generateAgentHalfExtents(float min, float max, Random& random) { @@ -94,12 +103,43 @@ namespace std::vector vertices; std::vector indices; std::vector areaTypes; - generateVertices(std::back_inserter(vertices), triangles * 1.946, random); - generateIndices(std::back_inserter(indices), static_cast(vertices.size() / 3) - 1, vertices.size() * 1.545, random); - generateAreaTypes(std::back_inserter(areaTypes), indices.size() / 3, random); + if (distribution(random) < 0.939) + { + generateVertices(std::back_inserter(vertices), triangles * 2.467, random); + generateIndices(std::back_inserter(indices), static_cast(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 + Heightfield generateHeightfield(Random& random) + { + std::uniform_real_distribution 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(ESM::Land::LAND_SIZE); + std::generate_n(std::back_inserter(result.mHeights), ESM::Land::LAND_NUM_VERTS, [&] + { + return distribution(random); + }); + return result; + } + + template + FlatHeightfield generateFlatHeightfield(Random& random) + { + std::uniform_real_distribution distribution(0.0, 1.0); + FlatHeightfield result; + result.mBounds = generateTileBounds(random); + result.mHeight = distribution(random); + return result; + } + template Key generateKey(std::size_t triangles, Random& random) { @@ -110,11 +150,12 @@ namespace Mesh mesh = generateMesh(triangles, random); std::vector 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 void generateKeys(OutputIterator out, std::size_t count, Random& random) diff --git a/apps/openmw/mwworld/scene.cpp b/apps/openmw/mwworld/scene.cpp index 61ef3e1538..1dc0f904bc 100644 --- a/apps/openmw/mwworld/scene.cpp +++ b/apps/openmw/mwworld/scene.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #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 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(ESM::Land::DEFAULT_HEIGHT)}; + } + else + { + DetourNavigator::HeightfieldSurface heights; + heights.mHeights = data->mHeights; + heights.mSize = static_cast(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().search(*cell->getCell())) diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp index b815c50bbe..a75275079b 100644 --- a/apps/openmw_test_suite/detournavigator/navigator.cpp +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -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 + HeightfieldSurface makeSquareHeightfieldSurface(const std::array& 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(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 heightfieldData {{ + constexpr std::array 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); - shape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); 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 heightfieldData {{ + const std::array 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); - heightfieldShape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); 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 heightfieldData {{ + const std::array 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); - heightfieldShape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); 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 heightfieldData {{ + const std::array 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 heightfieldData2 {{ @@ -383,6 +397,31 @@ namespace )) << mPath; } + TEST_F(DetourNavigatorNavigatorTest, only_one_heightfield_per_cell_is_allowed) + { + const std::array 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 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 heightfieldData {{ @@ -441,19 +480,18 @@ namespace TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_ground_lower_than_water_with_only_swim_flag) { - std::array heightfieldData {{ + std::array 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); - shape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); 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 heightfieldData {{ + std::array 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); - shape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, osg::Vec3f(0, 0, -25)); - mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->addWater(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), osg::Vec3f(0, 0, -25)); + 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 heightfieldData {{ + std::array 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); - shape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); 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::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 heightfieldData {{ + std::array 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); - shape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); 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 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 heightfieldData {{ + const std::array 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); - shape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); 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 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 heightfieldData {{ + const std::array 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); - heightfieldShape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); const std::vector 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 heightfieldData {{ + const std::array 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); - shape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); 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 heightfieldData {{ + const std::array 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); - heightfieldShape.setLocalScaling(btVector3(128, 128, 1)); + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); 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; + } } diff --git a/apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp b/apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp index 8c66f1a3ef..9405bd7739 100644 --- a/apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp +++ b/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 mWater {}; - const RecastMesh mRecastMesh {mGeneration, mRevision, mMesh, mWater}; + const std::vector mHeightfields {}; + const std::vector mFlatHeightfields {}; + const RecastMesh mRecastMesh {mGeneration, mRevision, mMesh, mWater, mHeightfields, mFlatHeightfields}; std::unique_ptr 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 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 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 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 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 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 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 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 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 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 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 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 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)); diff --git a/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp b/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp index b6478f9ea6..73d86bd6ee 100644 --- a/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp +++ b/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp @@ -3,12 +3,16 @@ #include #include #include +#include +#include +#include #include #include #include #include #include +#include #include @@ -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({2, 1, 0, 2, 1, 3})); EXPECT_EQ(recastMesh->getMesh().getAreaTypes(), std::vector({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 {TileBounds {osg::Vec2f(0, 0), osg::Vec2f(501, 502)}, 13}, + })); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_heightfield_inside_tile) + { + constexpr std::array 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({expected})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_heightfield_should_add_intersection) + { + constexpr std::array 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({expected})); + } } diff --git a/components/detournavigator/cachedrecastmeshmanager.cpp b/components/detournavigator/cachedrecastmeshmanager.cpp index cfbb77f238..154da806d7 100644 --- a/components/detournavigator/cachedrecastmeshmanager.cpp +++ b/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 CachedRecastMeshManager::removeHeightfield(const osg::Vec2i& cellPosition) + { + const auto cell = mImpl.removeHeightfield(cellPosition); + if (cell) + mCached.reset(); + return cell; + } + std::shared_ptr CachedRecastMeshManager::getMesh() { if (!mCached) diff --git a/components/detournavigator/cachedrecastmeshmanager.hpp b/components/detournavigator/cachedrecastmeshmanager.hpp index 83eb5c1ec3..ab7bb9e7cb 100644 --- a/components/detournavigator/cachedrecastmeshmanager.hpp +++ b/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 removeObject(const ObjectId id); + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const osg::Vec3f& shift); std::optional removeWater(const osg::Vec2i& cellPosition); - std::optional removeObject(const ObjectId id); + bool addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift, + const HeightfieldShape& shape); + + std::optional removeHeightfield(const osg::Vec2i& cellPosition); std::shared_ptr getMesh(); diff --git a/components/detournavigator/heightfieldshape.hpp b/components/detournavigator/heightfieldshape.hpp new file mode 100644 index 0000000000..48a273725b --- /dev/null +++ b/components/detournavigator/heightfieldshape.hpp @@ -0,0 +1,25 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_HEIGHFIELDSHAPE_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_HEIGHFIELDSHAPE_H + +#include +#include + +namespace DetourNavigator +{ + struct HeightfieldPlane + { + float mHeight; + }; + + struct HeightfieldSurface + { + const float* mHeights; + std::size_t mSize; + float mMinHeight; + float mMaxHeight; + }; + + using HeightfieldShape = std::variant; +} + +#endif diff --git a/components/detournavigator/makenavmesh.cpp b/components/detournavigator/makenavmesh.cpp index 797d73667b..07e9c7da66 100644 --- a/components/detournavigator/makenavmesh.cpp +++ b/components/detournavigator/makenavmesh.cpp @@ -9,8 +9,10 @@ #include "navmeshtilescache.hpp" #include "preparednavmeshdata.hpp" #include "navmeshdata.hpp" +#include "recastmeshbuilder.hpp" #include +#include #include #include @@ -195,64 +197,92 @@ namespace ); } - void rasterizeWaterTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const std::vector& 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(vertices.size() / 3), + indices.data(), + areas, + static_cast(areasSize), + solid, + config.walkableClimb + ); + } + + bool rasterizeTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const std::vector& cells, const Settings& settings, const rcConfig& config, rcHeightfield& solid) { const std::array areas {{AreaType_water, AreaType_water}}; - for (const Cell& cell : cells) { - const auto rectangle = getSwimRectangle(cell, settings, agentHalfExtents); - - const osg::Vec2f tileBoundsMin( - 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())) - ); - 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 { - 0, 1, 2, - 0, 2, 3, - }; - - const auto trianglesRasterized = rcRasterizeTriangles( - &context, - vertices.data(), - static_cast(vertices.size() / 3), - indices.data(), - areas.data(), - static_cast(areas.size()), - solid, - config.walkableClimb - ); - - if (!trianglesRasterized) - throw NavigatorException("Failed to create rasterize water triangles for navmesh"); + const Rectangle rectangle = getSwimRectangle(cell, settings, agentHalfExtents); + if (!rasterizeTriangles(context, rectangle, config, areas.data(), areas.size(), solid)) + return false; } + return true; + } + + bool rasterizeTriangles(rcContext& context, const std::vector& heightfields, + const Settings& settings, const rcConfig& config, rcHeightfield& solid) + { + for (const FlatHeightfield& heightfield : heightfields) + { + const std::array 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& 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 (!rasterizeTriangles(context, recastMesh.getMesh(), settings, config, solid)) - return false; - - rasterizeWaterTriangles(context, agentHalfExtents, recastMesh.getWater(), settings, config, solid); - - return true; + return rasterizeTriangles(context, recastMesh.getMesh(), settings, config, solid) + && rasterizeTriangles(context, agentHalfExtents, recastMesh.getWater(), settings, config, solid) + && rasterizeTriangles(context, recastMesh.getHeightfields(), settings, config, solid) + && rasterizeTriangles(context, recastMesh.getFlatHeightfields(), settings, config, solid); } void buildCompactHeightfield(rcContext& context, const int walkableHeight, const int walkableClimb, diff --git a/components/detournavigator/navigator.hpp b/components/detournavigator/navigator.hpp index f7ee4ed96e..6e0f773fb6 100644 --- a/components/detournavigator/navigator.hpp +++ b/components/detournavigator/navigator.hpp @@ -8,6 +8,9 @@ #include "navmeshcacheitem.hpp" #include "recastmeshtiles.hpp" #include "waitconditiontype.hpp" +#include "heightfieldshape.hpp" + +#include 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; diff --git a/components/detournavigator/navigatorimpl.cpp b/components/detournavigator/navigatorimpl.cpp index a6e1062fbb..e256d8f76f 100644 --- a/components/detournavigator/navigatorimpl.cpp +++ b/components/detournavigator/navigatorimpl.cpp @@ -2,6 +2,7 @@ #include "debug.hpp" #include "settingsutils.hpp" +#include #include #include @@ -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); diff --git a/components/detournavigator/navigatorimpl.hpp b/components/detournavigator/navigatorimpl.hpp index 3d021ec7e9..35a6888551 100644 --- a/components/detournavigator/navigatorimpl.hpp +++ b/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; diff --git a/components/detournavigator/navigatorstub.hpp b/components/detournavigator/navigatorstub.hpp index cab8feaf39..2d2fc936ca 100644 --- a/components/detournavigator/navigatorstub.hpp +++ b/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 {} diff --git a/components/detournavigator/navmeshmanager.cpp b/components/detournavigator/navmeshmanager.cpp index f2dd814d46..287a3c8ef9 100644 --- a/components/detournavigator/navmeshmanager.cpp +++ b/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); diff --git a/components/detournavigator/navmeshmanager.hpp b/components/detournavigator/navmeshmanager.hpp index a93d8824e3..390ba41ec8 100644 --- a/components/detournavigator/navmeshmanager.hpp +++ b/components/detournavigator/navmeshmanager.hpp @@ -6,6 +6,7 @@ #include "offmeshconnectionsmanager.hpp" #include "recastmeshtiles.hpp" #include "waitconditiontype.hpp" +#include "heightfieldshape.hpp" #include @@ -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); diff --git a/components/detournavigator/navmeshtilescache.cpp b/components/detournavigator/navmeshtilescache.cpp index c4f81727ab..4710a0a4f6 100644 --- a/components/detournavigator/navmeshtilescache.cpp +++ b/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); diff --git a/components/detournavigator/navmeshtilescache.hpp b/components/detournavigator/navmeshtilescache.hpp index d218ffdcee..06d6c69e9b 100644 --- a/components/detournavigator/navmeshtilescache.hpp +++ b/components/detournavigator/navmeshtilescache.hpp @@ -24,24 +24,26 @@ namespace DetourNavigator { Mesh mMesh; std::vector mWater; + std::vector mHeightfields; + std::vector mFlatHeightfields; }; inline bool operator <(const RecastMeshData& lhs, const RecastMeshData& rhs) { - return std::tie(lhs.mMesh, lhs.mWater) - < std::tie(rhs.mMesh, rhs.mWater); + return std::tie(lhs.mMesh, lhs.mWater, lhs.mHeightfields, lhs.mFlatHeightfields) + < 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) - < std::tie(rhs.getMesh(), rhs.getWater()); + return std::tie(lhs.mMesh, lhs.mWater, lhs.mHeightfields, lhs.mFlatHeightfields) + < 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()) - < std::tie(rhs.mMesh, rhs.mWater); + return std::tie(lhs.getMesh(), lhs.getWater(), lhs.getHeightfields(), lhs.getFlatHeightfields()) + < std::tie(rhs.mMesh, rhs.mWater, rhs.mHeightfields, rhs.mFlatHeightfields); } class NavMeshTilesCache diff --git a/components/detournavigator/recastmesh.cpp b/components/detournavigator/recastmesh.cpp index bf7a97caa8..e2dea0ad6e 100644 --- a/components/detournavigator/recastmesh.cpp +++ b/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 water) + RecastMesh::RecastMesh(std::size_t generation, std::size_t revision, Mesh mesh, std::vector water, + std::vector heightfields, std::vector 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(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); + } } } diff --git a/components/detournavigator/recastmesh.hpp b/components/detournavigator/recastmesh.hpp index f316abf653..c8e160603b 100644 --- a/components/detournavigator/recastmesh.hpp +++ b/components/detournavigator/recastmesh.hpp @@ -3,6 +3,7 @@ #include "areatype.hpp" #include "bounds.hpp" +#include "tilebounds.hpp" #include @@ -12,6 +13,7 @@ #include #include #include +#include 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 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 water); + RecastMesh(std::size_t generation, std::size_t revision, Mesh mesh, std::vector water, + std::vector heightfields, std::vector flatHeightfields); std::size_t getGeneration() const { @@ -73,6 +108,16 @@ namespace DetourNavigator return mWater; } + const std::vector& getHeightfields() const noexcept + { + return mHeightfields; + } + + const std::vector& 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 mWater; + std::vector mHeightfields; + std::vector 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); } }; diff --git a/components/detournavigator/recastmeshbuilder.cpp b/components/detournavigator/recastmeshbuilder.cpp index b47d823199..6ee16cdc6c 100644 --- a/components/detournavigator/recastmeshbuilder.cpp +++ b/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&& triangles) + Mesh makeMesh(std::vector&& triangles, const osg::Vec3f& shift) { std::vector 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.y()); - vertices.push_back(v.z()); + 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 heights(heightfield.mHeights.begin(), heightfield.mHeights.end()); + btHeightfieldTerrainShape shape(static_cast(heightfield.mHeights.size() / heightfield.mLength), + static_cast(heightfield.mLength), heights.data(), 1, + heightfield.mMinHeight, heightfield.mMaxHeight, upAxis, PHY_FLOAT, flipQuadEdges + ); +#else + btHeightfieldTerrainShape shape(static_cast(heightfield.mHeights.size() / heightfield.mLength), + static_cast(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 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(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(static_cast(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 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(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 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(generation, revision, std::move(mesh), std::move(mWater)); + return std::make_shared(generation, revision, std::move(mesh), std::move(mWater), + std::move(mHeightfields), std::move(mFlatHeightfields)); } void RecastMeshBuilder::addObject(const btConcaveShape& shape, const btTransform& transform, diff --git a/components/detournavigator/recastmeshbuilder.hpp b/components/detournavigator/recastmeshbuilder.hpp index d08ff94418..120e4b045a 100644 --- a/components/detournavigator/recastmeshbuilder.hpp +++ b/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 create(std::size_t generation, std::size_t revision) &&; private: const TileBounds mBounds; std::vector mTriangles; std::vector mWater; + std::vector mHeightfields; + std::vector 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&& triangles); + Mesh makeMesh(std::vector&& triangles, const osg::Vec3f& shift = osg::Vec3f()); + + Mesh makeMesh(const Heightfield& heightfield); } #endif diff --git a/components/detournavigator/recastmeshmanager.cpp b/components/detournavigator/recastmeshmanager.cpp index a6798ee8cd..151b3161c1 100644 --- a/components/detournavigator/recastmeshmanager.cpp +++ b/components/detournavigator/recastmeshmanager.cpp @@ -1,6 +1,30 @@ #include "recastmeshmanager.hpp" #include "recastmeshbuilder.hpp" #include "settings.hpp" +#include "heightfieldshape.hpp" + +#include + +#include + +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 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 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) diff --git a/components/detournavigator/recastmeshmanager.hpp b/components/detournavigator/recastmeshmanager.hpp index 266d6a9385..88124c44cf 100644 --- a/components/detournavigator/recastmeshmanager.hpp +++ b/components/detournavigator/recastmeshmanager.hpp @@ -5,6 +5,7 @@ #include "objectid.hpp" #include "version.hpp" #include "recastmesh.hpp" +#include "heightfieldshape.hpp" #include @@ -13,6 +14,8 @@ #include #include #include +#include +#include class btCollisionShape; @@ -37,11 +40,16 @@ namespace DetourNavigator bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType); + std::optional removeObject(const ObjectId id); + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const osg::Vec3f& shift); std::optional removeWater(const osg::Vec2i& cellPosition); - std::optional removeObject(const ObjectId id); + bool addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift, + const HeightfieldShape& shape); + + std::optional removeHeightfield(const osg::Vec2i& cellPosition); std::shared_ptr 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 mObjects; std::map mWater; + std::map mHeightfields; std::optional mLastNavMeshReportedChange; std::optional mLastNavMeshReport; }; diff --git a/components/detournavigator/tilebounds.hpp b/components/detournavigator/tilebounds.hpp index 83fe2b6296..8557045342 100644 --- a/components/detournavigator/tilebounds.hpp +++ b/components/detournavigator/tilebounds.hpp @@ -3,6 +3,10 @@ #include +#include +#include +#include + 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 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 diff --git a/components/detournavigator/tilecachedrecastmeshmanager.cpp b/components/detournavigator/tilecachedrecastmeshmanager.cpp index 1292e31d4e..a37d24399d 100644 --- a/components/detournavigator/tilecachedrecastmeshmanager.cpp +++ b/components/detournavigator/tilecachedrecastmeshmanager.cpp @@ -3,6 +3,8 @@ #include "gettilespositions.hpp" #include "settingsutils.hpp" +#include + #include #include @@ -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 TileCachedRecastMeshManager::removeHeightfield(const osg::Vec2i& cellPosition) + { + const auto object = mHeightfieldTilesPositions.find(cellPosition); + if (object == mHeightfieldTilesPositions.end()) + return std::nullopt; + std::optional 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 TileCachedRecastMeshManager::getMesh(const TilePosition& tilePosition) { const auto tiles = mTiles.lock(); diff --git a/components/detournavigator/tilecachedrecastmeshmanager.hpp b/components/detournavigator/tilecachedrecastmeshmanager.hpp index 4ea705f4cd..f3292cbf9b 100644 --- a/components/detournavigator/tilecachedrecastmeshmanager.hpp +++ b/components/detournavigator/tilecachedrecastmeshmanager.hpp @@ -6,6 +6,7 @@ #include "settingsutils.hpp" #include "gettilespositions.hpp" #include "version.hpp" +#include "heightfieldshape.hpp" #include @@ -80,6 +81,11 @@ namespace DetourNavigator std::optional removeWater(const osg::Vec2i& cellPosition); + bool addHeightfield(const osg::Vec2i& cellPosition, int cellSize, const osg::Vec3f& shift, + const HeightfieldShape& shape); + + std::optional removeHeightfield(const osg::Vec2i& cellPosition); + std::shared_ptr getMesh(const TilePosition& tilePosition); bool hasTile(const TilePosition& tilePosition); @@ -100,6 +106,7 @@ namespace DetourNavigator Misc::ScopeGuarded> mTiles; std::unordered_map> mObjectsTilesPositions; std::map> mWaterTilesPositions; + std::map> mHeightfieldTilesPositions; std::size_t mRevision = 0; std::size_t mTilesGeneration = 0; diff --git a/components/sceneutil/recastmesh.cpp b/components/sceneutil/recastmesh.cpp index 556fc3f3b5..85f3e7177b 100644 --- a/components/sceneutil/recastmesh.cpp +++ b/components/sceneutil/recastmesh.cpp @@ -3,6 +3,7 @@ #include #include +#include #include @@ -45,15 +46,25 @@ namespace SceneUtil const osg::ref_ptr group(new osg::Group); DebugDraw debugDraw(*group, osg::Vec3f(0, 0, 0), 1.0f); const DetourNavigator::Mesh& mesh = recastMesh.getMesh(); + std::vector indices = mesh.getIndices(); std::vector vertices = mesh.getVertices(); + for (const Heightfield& heightfield : recastMesh.getHeightfields()) + { + const Mesh mesh = makeMesh(heightfield); + const int indexShift = static_cast(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(vertices.size() / 3), - mesh.getIndices().data(), normals.data(), static_cast(mesh.getIndices().size() / 3), nullptr, texScale); + indices.data(), normals.data(), static_cast(indices.size() / 3), nullptr, texScale); return group; } }