From fafba8ea0c6b0efc4af68ac6a726c86d061ede17 Mon Sep 17 00:00:00 2001 From: elsid Date: Wed, 14 Mar 2018 01:49:08 +0300 Subject: [PATCH] Use recastnavigation to find path --- apps/openmw/mwbase/world.hpp | 7 + apps/openmw/mwmechanics/aipackage.hpp | 2 + apps/openmw/mwmechanics/pathfinding.cpp | 25 ++ apps/openmw/mwmechanics/pathfinding.hpp | 3 + apps/openmw/mwphysics/physicssystem.cpp | 23 +- apps/openmw/mwphysics/physicssystem.hpp | 6 +- apps/openmw/mwworld/cellstore.hpp | 2 +- apps/openmw/mwworld/scene.cpp | 54 +++- apps/openmw/mwworld/worldimp.cpp | 27 ++ apps/openmw/mwworld/worldimp.hpp | 3 + apps/openmw_test_suite/CMakeLists.txt | 8 + .../detournavigator/navigator.cpp | 159 ++++++++++ .../detournavigator/operators.hpp | 40 +++ .../detournavigator/recastmeshbuilder.cpp | 81 +++++ .../detournavigator/settingsutils.cpp | 68 +++++ components/CMakeLists.txt | 19 ++ .../detournavigator/asyncnavmeshupdater.cpp | 113 +++++++ .../detournavigator/asyncnavmeshupdater.hpp | 75 +++++ .../cachedrecastmeshmanager.cpp | 25 ++ .../cachedrecastmeshmanager.hpp | 34 +++ components/detournavigator/chunkytrimesh.cpp | 174 +++++++++++ components/detournavigator/chunkytrimesh.hpp | 93 ++++++ components/detournavigator/debug.cpp | 163 ++++++++++ components/detournavigator/debug.hpp | 63 ++++ components/detournavigator/dtstatus.hpp | 66 ++++ components/detournavigator/exceptions.hpp | 15 + components/detournavigator/findsmoothpath.cpp | 143 +++++++++ components/detournavigator/findsmoothpath.hpp | 266 ++++++++++++++++ components/detournavigator/makenavmesh.cpp | 288 ++++++++++++++++++ components/detournavigator/makenavmesh.hpp | 20 ++ components/detournavigator/navigator.cpp | 42 +++ components/detournavigator/navigator.hpp | 50 +++ components/detournavigator/navmeshmanager.cpp | 50 +++ components/detournavigator/navmeshmanager.hpp | 48 +++ components/detournavigator/recastmesh.cpp | 12 + components/detournavigator/recastmesh.hpp | 50 +++ .../detournavigator/recastmeshbuilder.cpp | 72 +++++ .../detournavigator/recastmeshbuilder.hpp | 38 +++ .../detournavigator/recastmeshmanager.cpp | 57 ++++ .../detournavigator/recastmeshmanager.hpp | 42 +++ components/detournavigator/settings.hpp | 30 ++ components/detournavigator/settingsutils.hpp | 65 ++++ components/detournavigator/tilebounds.hpp | 15 + components/detournavigator/tileposition.hpp | 11 + files/settings-default.cfg | 60 ++++ 45 files changed, 2698 insertions(+), 9 deletions(-) create mode 100644 apps/openmw_test_suite/detournavigator/navigator.cpp create mode 100644 apps/openmw_test_suite/detournavigator/operators.hpp create mode 100644 apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp create mode 100644 apps/openmw_test_suite/detournavigator/settingsutils.cpp create mode 100644 components/detournavigator/asyncnavmeshupdater.cpp create mode 100644 components/detournavigator/asyncnavmeshupdater.hpp create mode 100644 components/detournavigator/cachedrecastmeshmanager.cpp create mode 100644 components/detournavigator/cachedrecastmeshmanager.hpp create mode 100644 components/detournavigator/chunkytrimesh.cpp create mode 100644 components/detournavigator/chunkytrimesh.hpp create mode 100644 components/detournavigator/debug.cpp create mode 100644 components/detournavigator/debug.hpp create mode 100644 components/detournavigator/dtstatus.hpp create mode 100644 components/detournavigator/exceptions.hpp create mode 100644 components/detournavigator/findsmoothpath.cpp create mode 100644 components/detournavigator/findsmoothpath.hpp create mode 100644 components/detournavigator/makenavmesh.cpp create mode 100644 components/detournavigator/makenavmesh.hpp create mode 100644 components/detournavigator/navigator.cpp create mode 100644 components/detournavigator/navigator.hpp create mode 100644 components/detournavigator/navmeshmanager.cpp create mode 100644 components/detournavigator/navmeshmanager.hpp create mode 100644 components/detournavigator/recastmesh.cpp create mode 100644 components/detournavigator/recastmesh.hpp create mode 100644 components/detournavigator/recastmeshbuilder.cpp create mode 100644 components/detournavigator/recastmeshbuilder.hpp create mode 100644 components/detournavigator/recastmeshmanager.cpp create mode 100644 components/detournavigator/recastmeshmanager.hpp create mode 100644 components/detournavigator/settings.hpp create mode 100644 components/detournavigator/settingsutils.hpp create mode 100644 components/detournavigator/tilebounds.hpp create mode 100644 components/detournavigator/tileposition.hpp diff --git a/apps/openmw/mwbase/world.hpp b/apps/openmw/mwbase/world.hpp index e17935abcd..18f997324f 100644 --- a/apps/openmw/mwbase/world.hpp +++ b/apps/openmw/mwbase/world.hpp @@ -54,6 +54,11 @@ namespace MWMechanics struct Movement; } +namespace DetourNavigator +{ + class Navigator; +} + namespace MWWorld { class CellStore; @@ -590,6 +595,8 @@ namespace MWBase /// Preload VFX associated with this effect list virtual void preloadEffects(const ESM::EffectList* effectList) = 0; + + virtual DetourNavigator::Navigator* getNavigator() const = 0; }; } diff --git a/apps/openmw/mwmechanics/aipackage.hpp b/apps/openmw/mwmechanics/aipackage.hpp index 440cf64b5e..59bd6689d5 100644 --- a/apps/openmw/mwmechanics/aipackage.hpp +++ b/apps/openmw/mwmechanics/aipackage.hpp @@ -7,6 +7,8 @@ #include "obstacle.hpp" #include "aistate.hpp" +#include + namespace MWWorld { class Ptr; diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index e0a83910f3..5a34bb2300 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -1,7 +1,13 @@ #include "pathfinding.hpp" +#include #include +#include +#include +#include +#include + #include "../mwbase/world.hpp" #include "../mwbase/environment.hpp" @@ -282,4 +288,23 @@ namespace MWMechanics } } } + + void PathFinder::buildPathByNavigator(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const osg::Vec3f& halfExtents) + { + try + { + mPath.clear(); + + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + navigator->findPath(halfExtents, startPoint, endPoint, std::back_inserter(mPath)); + + mConstructed = true; + } + catch (const DetourNavigator::NavigatorException& exception) + { + DetourNavigator::log("PathFinder::buildPathByNavigator navigator exception: ", exception.what()); + Log(Debug::Error) << "Build path by navigator exception: " << exception.what(); + } + } } diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index 419b185852..f488e3b147 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -175,6 +175,9 @@ namespace MWMechanics return closestIndex; } + void buildPathByNavigator(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const osg::Vec3f& halfExtents); + private: bool mConstructed; std::deque mPath; diff --git a/apps/openmw/mwphysics/physicssystem.cpp b/apps/openmw/mwphysics/physicssystem.cpp index 74354e101c..c90dcbf98b 100644 --- a/apps/openmw/mwphysics/physicssystem.cpp +++ b/apps/openmw/mwphysics/physicssystem.cpp @@ -1,6 +1,11 @@ -#include "physicssystem.hpp" +#include "physicssystem.hpp" #include +#include +#include +#include + +#include #include @@ -16,6 +21,12 @@ #include +#include +#include +#include +#include +#include + #include #include #include @@ -54,8 +65,6 @@ namespace MWPhysics { - static const float sMaxSlope = 49.0f; - static const float sStepSizeUp = 34.0f; static const float sStepSizeDown = 62.0f; static const float sMinStep = 10.f; static const float sGroundOffset = 1.0f; @@ -1004,6 +1013,14 @@ namespace MWPhysics } } + const HeightField* PhysicsSystem::getHeightField(int x, int y) const + { + const auto heightField = mHeightFields.find(std::make_pair(x, y)); + if (heightField == mHeightFields.end()) + return nullptr; + return heightField->second; + } + void PhysicsSystem::addObject (const MWWorld::Ptr& ptr, const std::string& mesh, int collisionType) { osg::ref_ptr shapeInstance = mShapeManager->getInstance(mesh); diff --git a/apps/openmw/mwphysics/physicssystem.hpp b/apps/openmw/mwphysics/physicssystem.hpp index 3ef9990f53..b084cc4c9e 100644 --- a/apps/openmw/mwphysics/physicssystem.hpp +++ b/apps/openmw/mwphysics/physicssystem.hpp @@ -49,6 +49,9 @@ namespace MWPhysics class Object; class Actor; + static const float sMaxSlope = 49.0f; + static const float sStepSizeUp = 34.0f; + class PhysicsSystem { public: @@ -85,6 +88,8 @@ namespace MWPhysics void removeHeightField (int x, int y); + const HeightField* getHeightField(int x, int y) const; + bool toggleCollisionMode(); void stepSimulation(float dt); @@ -169,7 +174,6 @@ namespace MWPhysics void markAsNonSolid (const MWWorld::ConstPtr& ptr); bool isOnSolidGround (const MWWorld::Ptr& actor) const; - private: void updateWater(); diff --git a/apps/openmw/mwworld/cellstore.hpp b/apps/openmw/mwworld/cellstore.hpp index f4d4305214..49bc0b3639 100644 --- a/apps/openmw/mwworld/cellstore.hpp +++ b/apps/openmw/mwworld/cellstore.hpp @@ -283,7 +283,7 @@ namespace MWWorld /// \attention This function also lists deleted (count 0) objects! /// \return Iteration completed? template - bool forEachConst (Visitor& visitor) const + bool forEachConst (Visitor&& visitor) const { if (mState != State_Loaded) return false; diff --git a/apps/openmw/mwworld/scene.cpp b/apps/openmw/mwworld/scene.cpp index 2d629adb58..2626bc641c 100644 --- a/apps/openmw/mwworld/scene.cpp +++ b/apps/openmw/mwworld/scene.cpp @@ -2,12 +2,16 @@ #include +#include + #include #include #include #include #include #include +#include +#include #include "../mwbase/environment.hpp" #include "../mwbase/world.hpp" @@ -19,6 +23,9 @@ #include "../mwrender/landmanager.hpp" #include "../mwphysics/physicssystem.hpp" +#include "../mwphysics/actor.hpp" +#include "../mwphysics/object.hpp" +#include "../mwphysics/heightfield.hpp" #include "player.hpp" #include "localscripts.hpp" @@ -74,6 +81,21 @@ namespace ptr.getClass().insertObject (ptr, model, physics); + if (const auto object = physics.getObject(ptr)) + { + if (const auto concaveShape = dynamic_cast(object->getShapeInstance()->mCollisionShape)) + { + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + navigator->addObject(reinterpret_cast(object), *concaveShape, + object->getCollisionObject()->getWorldTransform()); + } + } + else if (const auto actor = physics.getActor(ptr)) + { + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + navigator->addAgent(actor->getHalfExtents()); + } + if (useAnim) MWBase::Environment::get().getMechanicsManager()->add(ptr); @@ -233,13 +255,18 @@ namespace MWWorld void Scene::unloadCell (CellStoreCollection::iterator iter) { Log(Debug::Info) << "Unloading cell " << (*iter)->getCell()->getDescription(); + + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); ListAndResetObjectsVisitor visitor; (*iter)->forEach(visitor); - for (std::vector::const_iterator iter2 (visitor.mObjects.begin()); - iter2!=visitor.mObjects.end(); ++iter2) + for (const auto& ptr : visitor.mObjects) { - mPhysics->remove(*iter2); + if (const auto object = mPhysics->getObject(ptr)) + navigator->removeObject(reinterpret_cast(object)); + else if (const auto actor = mPhysics->getActor(ptr)) + navigator->removeAgent(actor->getHalfExtents()); + mPhysics->remove(ptr); } if ((*iter)->getCell()->isExterior()) @@ -250,7 +277,13 @@ namespace MWWorld (*iter)->getCell()->getGridY() ); if (land && land->mDataTypes&ESM::Land::DATA_VHGT) - mPhysics->removeHeightField ((*iter)->getCell()->getGridX(), (*iter)->getCell()->getGridY()); + { + const auto cellX = (*iter)->getCell()->getGridX(); + const auto cellY = (*iter)->getCell()->getGridY(); + if (const auto heightField = mPhysics->getHeightField(cellX, cellY)) + navigator->removeObject(reinterpret_cast(heightField)); + mPhysics->removeHeightField(cellX, cellY); + } } MWBase::Environment::get().getMechanicsManager()->drop (*iter); @@ -275,6 +308,8 @@ namespace MWWorld float verts = ESM::Land::LAND_SIZE; float worldsize = ESM::Land::REAL_SIZE; + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + // Load terrain physics first... if (cell->getCell()->isExterior()) { @@ -292,6 +327,10 @@ namespace MWWorld defaultHeight.resize(verts*verts, ESM::Land::DEFAULT_HEIGHT); mPhysics->addHeightField (&defaultHeight[0], cell->getCell()->getGridX(), cell->getCell()->getGridY(), worldsize / (verts-1), verts, ESM::Land::DEFAULT_HEIGHT, ESM::Land::DEFAULT_HEIGHT, land.get()); } + + if (const auto heightField = mPhysics->getHeightField(cellX, cellY)) + navigator->addObject(reinterpret_cast(heightField), *heightField->getShape(), + heightField->getCollisionObject()->getWorldTransform()); } // register local scripts @@ -317,6 +356,8 @@ namespace MWWorld else mPhysics->disableWater(); + navigator->update(); + if (!cell->isExterior() && !(cell->getCell()->mData.mFlags & ESM::Cell::QuasiEx)) mRendering.configureAmbient(cell->getCell()); } @@ -631,6 +672,11 @@ namespace MWWorld { MWBase::Environment::get().getMechanicsManager()->remove (ptr); MWBase::Environment::get().getSoundManager()->stopSound3D (ptr); + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + if (const auto object = mPhysics->getObject(ptr)) + navigator->removeObject(reinterpret_cast(object)); + else if (const auto actor = mPhysics->getActor(ptr)) + navigator->removeAgent(actor->getHalfExtents()); mPhysics->remove(ptr); mRendering.removeObject (ptr); if (ptr.getClass().isActor()) diff --git a/apps/openmw/mwworld/worldimp.cpp b/apps/openmw/mwworld/worldimp.cpp index 0872e589dc..322559e3bd 100644 --- a/apps/openmw/mwworld/worldimp.cpp +++ b/apps/openmw/mwworld/worldimp.cpp @@ -19,6 +19,8 @@ #include +#include + #include "../mwbase/environment.hpp" #include "../mwbase/soundmanager.hpp" #include "../mwbase/mechanicsmanager.hpp" @@ -163,6 +165,26 @@ namespace MWWorld mRendering.reset(new MWRender::RenderingManager(viewer, rootNode, resourceSystem, workQueue, &mFallback, resourcePath)); mProjectileManager.reset(new ProjectileManager(mRendering->getLightRoot(), resourceSystem, mRendering.get(), mPhysics.get())); + DetourNavigator::Settings navigatorSettings; + navigatorSettings.mCellHeight = Settings::Manager::getFloat("cell height", "Navigator"); + navigatorSettings.mCellSize = Settings::Manager::getFloat("cell size", "Navigator"); + navigatorSettings.mDetailSampleDist = Settings::Manager::getFloat("detail sample dist", "Navigator"); + navigatorSettings.mDetailSampleMaxError = Settings::Manager::getFloat("detail sample max error", "Navigator"); + navigatorSettings.mMaxClimb = MWPhysics::sStepSizeUp; + navigatorSettings.mMaxSimplificationError = Settings::Manager::getFloat("max simplification error", "Navigator"); + navigatorSettings.mMaxSlope = MWPhysics::sMaxSlope; + navigatorSettings.mRecastScaleFactor = Settings::Manager::getFloat("recast scale factor", "Navigator"); + navigatorSettings.mMaxEdgeLen = Settings::Manager::getInt("max edge len", "Navigator"); + navigatorSettings.mMaxNavMeshQueryNodes = Settings::Manager::getInt("max nav mesh query nodes", "Navigator"); + navigatorSettings.mMaxVertsPerPoly = Settings::Manager::getInt("max verts per poly", "Navigator"); + navigatorSettings.mRegionMergeSize = Settings::Manager::getInt("region merge size", "Navigator"); + navigatorSettings.mRegionMinSize = Settings::Manager::getInt("region min size", "Navigator"); + navigatorSettings.mTileSize = Settings::Manager::getInt("tile size", "Navigator"); + navigatorSettings.mMaxPolygonPathSize = static_cast(Settings::Manager::getInt("max polygon path size", "Navigator")); + navigatorSettings.mMaxSmoothPathSize = static_cast(Settings::Manager::getInt("max smooth path size", "Navigator")); + navigatorSettings.mTrianglesPerChunk = static_cast(Settings::Manager::getInt("triangles per chunk", "Navigator")); + mNavigator.reset(new DetourNavigator::Navigator(navigatorSettings)); + mRendering->preloadCommonAssets(); mEsm.resize(contentFiles.size()); @@ -3684,4 +3706,9 @@ namespace MWWorld } } + DetourNavigator::Navigator* World::getNavigator() const + { + return mNavigator.get(); + } + } diff --git a/apps/openmw/mwworld/worldimp.hpp b/apps/openmw/mwworld/worldimp.hpp index 7df8d1af5b..4d264ff7f6 100644 --- a/apps/openmw/mwworld/worldimp.hpp +++ b/apps/openmw/mwworld/worldimp.hpp @@ -98,6 +98,7 @@ namespace MWWorld std::unique_ptr mWorldScene; std::unique_ptr mWeatherManager; std::shared_ptr mProjectileManager; + std::unique_ptr mNavigator; bool mGodMode; bool mScriptsEnabled; @@ -688,6 +689,8 @@ namespace MWWorld /// Preload VFX associated with this effect list void preloadEffects(const ESM::EffectList* effectList) override; + + DetourNavigator::Navigator* getNavigator() const override; }; } diff --git a/apps/openmw_test_suite/CMakeLists.txt b/apps/openmw_test_suite/CMakeLists.txt index bc39bc2be6..57fb1a0128 100644 --- a/apps/openmw_test_suite/CMakeLists.txt +++ b/apps/openmw_test_suite/CMakeLists.txt @@ -17,10 +17,18 @@ if (GTEST_FOUND AND GMOCK_FOUND) misc/test_stringops.cpp nifloader/testbulletnifloader.cpp + + detournavigator/navigator.cpp + detournavigator/settingsutils.cpp + detournavigator/recastmeshbuilder.cpp ) source_group(apps\\openmw_test_suite FILES openmw_test_suite.cpp ${UNITTEST_SRC_FILES}) + find_package(RecastNavigation COMPONENTS DebugUtils Detour Recast REQUIRED) + + include_directories(SYSTEM ${RecastNavigation_INCLUDE_DIRS}) + openmw_add_executable(openmw_test_suite openmw_test_suite.cpp ${UNITTEST_SRC_FILES}) target_link_libraries(openmw_test_suite ${GMOCK_LIBRARIES} components) diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp new file mode 100644 index 0000000000..1a317da843 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -0,0 +1,159 @@ +#include "operators.hpp" + +#include +#include + +#include + +#include + +#include +#include + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorNavigatorTest : Test + { + Settings mSettings; + std::unique_ptr mNavigator; + osg::Vec3f mPlayerPosition; + osg::Vec3f mAgentHalfExtents; + osg::Vec3f mStart; + osg::Vec3f mEnd; + std::deque mPath; + std::back_insert_iterator> mOut; + + DetourNavigatorNavigatorTest() + : mPlayerPosition(0, 0, 0) + , mAgentHalfExtents(29, 29, 66) + , mStart(-215, 215, 1) + , mEnd(215, -215, 1) + , mOut(mPath) + { + mSettings.mCellHeight = 0.2f; + mSettings.mCellSize = 0.2f; + mSettings.mDetailSampleDist = 6; + mSettings.mDetailSampleMaxError = 1; + mSettings.mMaxClimb = 34; + mSettings.mMaxSimplificationError = 1.3f; + mSettings.mMaxSlope = 49; + mSettings.mRecastScaleFactor = 0.017647058823529415f; + mSettings.mMaxEdgeLen = 12; + mSettings.mMaxNavMeshQueryNodes = 2048; + mSettings.mMaxVertsPerPoly = 6; + mSettings.mRegionMergeSize = 20; + mSettings.mRegionMinSize = 8; + mSettings.mTileSize = 64; + mSettings.mMaxPolygonPathSize = 1024; + mSettings.mMaxSmoothPathSize = 1024; + mSettings.mTrianglesPerChunk = 256; + mNavigator.reset(new Navigator(mSettings)); + } + }; + + TEST_F(DetourNavigatorNavigatorTest, update_then_find_path_should_return_path) + { + 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(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(1, shape, btTransform::getIdentity()); + mNavigator->update(); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(-215, 215, 1.85963428020477294921875), + osg::Vec3f(-194.9653167724609375, 194.9653167724609375, -6.5760211944580078125), + osg::Vec3f(-174.930633544921875, 174.930633544921875, -15.01167774200439453125), + osg::Vec3f(-154.8959503173828125, 154.8959503173828125, -23.4473323822021484375), + osg::Vec3f(-134.86126708984375, 134.86126708984375, -31.8829898834228515625), + osg::Vec3f(-114.82657623291015625, 114.82657623291015625, -40.3186492919921875), + osg::Vec3f(-94.7918853759765625, 94.7918853759765625, -47.39907073974609375), + osg::Vec3f(-74.75719451904296875, 74.75719451904296875, -53.7258148193359375), + osg::Vec3f(-54.722499847412109375, 54.722499847412109375, -60.052555084228515625), + osg::Vec3f(-34.68780517578125, 34.68780517578125, -66.37929534912109375), + osg::Vec3f(-14.6531162261962890625, 14.6531162261962890625, -72.70604705810546875), + osg::Vec3f(5.3815765380859375, -5.3815765380859375, -75.35065460205078125), + osg::Vec3f(25.41626739501953125, -25.41626739501953125, -67.96945953369140625), + osg::Vec3f(45.450958251953125, -45.450958251953125, -60.58824920654296875), + osg::Vec3f(65.48564910888671875, -65.48564910888671875, -53.20705413818359375), + osg::Vec3f(85.5203399658203125, -85.5203399658203125, -45.825855255126953125), + osg::Vec3f(105.55503082275390625, -105.55503082275390625, -38.44464874267578125), + osg::Vec3f(125.5897216796875, -125.5897216796875, -31.063449859619140625), + osg::Vec3f(145.6244049072265625, -145.6244049072265625, -23.6822509765625), + osg::Vec3f(165.659088134765625, -165.659088134765625, -16.3010540008544921875), + osg::Vec3f(185.6937713623046875, -185.6937713623046875, -8.91985416412353515625), + osg::Vec3f(205.7284698486328125, -205.7284698486328125, -1.53864824771881103515625), + osg::Vec3f(215, -215, 1.877177715301513671875), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, for_overlapping_heightfields_should_use_higher) + { + 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(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape.setLocalScaling(btVector3(128, 128, 1)); + + 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, + }}; + btHeightfieldTerrainShape shape2(5, 5, heightfieldData2.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape2.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(1, shape, btTransform::getIdentity()); + mNavigator->addObject(2, shape2, btTransform::getIdentity()); + mNavigator->update(); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(-215, 215, 1.96328866481781005859375), + osg::Vec3f(-194.9653167724609375, 194.9653167724609375, -0.2422157227993011474609375), + osg::Vec3f(-174.930633544921875, 174.930633544921875, -2.44772052764892578125), + osg::Vec3f(-154.8959503173828125, 154.8959503173828125, -4.653223514556884765625), + osg::Vec3f(-134.86126708984375, 134.86126708984375, -6.858728885650634765625), + osg::Vec3f(-114.82657623291015625, 114.82657623291015625, -9.0642337799072265625), + osg::Vec3f(-94.7918853759765625, 94.7918853759765625, -11.26973724365234375), + osg::Vec3f(-74.75719451904296875, 74.75719451904296875, -13.26497173309326171875), + osg::Vec3f(-54.722499847412109375, 54.722499847412109375, -15.24860286712646484375), + osg::Vec3f(-34.68780517578125, 34.68780517578125, -17.2322368621826171875), + osg::Vec3f(-14.6531162261962890625, 14.6531162261962890625, -19.2158660888671875), + osg::Vec3f(5.3815765380859375, -5.3815765380859375, -20.1338443756103515625), + osg::Vec3f(25.41626739501953125, -25.41626739501953125, -18.150211334228515625), + osg::Vec3f(45.450958251953125, -45.450958251953125, -16.1665802001953125), + osg::Vec3f(65.48564910888671875, -65.48564910888671875, -14.18294811248779296875), + osg::Vec3f(85.5203399658203125, -85.5203399658203125, -12.19931507110595703125), + osg::Vec3f(105.55503082275390625, -105.55503082275390625, -10.08488559722900390625), + osg::Vec3f(125.5897216796875, -125.5897216796875, -7.879383563995361328125), + osg::Vec3f(145.6244049072265625, -145.6244049072265625, -5.673877239227294921875), + osg::Vec3f(165.659088134765625, -165.659088134765625, -3.4683735370635986328125), + osg::Vec3f(185.6937713623046875, -185.6937713623046875, -1.2628715038299560546875), + osg::Vec3f(205.7284698486328125, -205.7284698486328125, 0.9426348209381103515625), + osg::Vec3f(215, -215, 1.96328866481781005859375), + })) << mPath; + } +} diff --git a/apps/openmw_test_suite/detournavigator/operators.hpp b/apps/openmw_test_suite/detournavigator/operators.hpp new file mode 100644 index 0000000000..8e6b97af6f --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/operators.hpp @@ -0,0 +1,40 @@ +#ifndef OPENMW_TEST_SUITE_DETOURNAVIGATOR_OPERATORS_H +#define OPENMW_TEST_SUITE_DETOURNAVIGATOR_OPERATORS_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace DetourNavigator +{ + static inline bool operator ==(const TileBounds& lhs, const TileBounds& rhs) + { + return lhs.mMin == rhs.mMin && lhs.mMax == rhs.mMax; + } +} + +namespace testing +{ + template <> + inline testing::Message& Message::operator <<(const std::deque& value) + { + (*this) << "{\n"; + for (const auto& v : value) + { + std::ostringstream stream; + stream << v; + (*this) << stream.str() << ",\n"; + } + return (*this) << "}"; + } +} + +#endif diff --git a/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp b/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp new file mode 100644 index 0000000000..ff72bc3eb8 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp @@ -0,0 +1,81 @@ +#include "operators.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorRecastMeshBuilderTest : Test + { + Settings mSettings; + RecastMeshBuilder mBuilder; + + DetourNavigatorRecastMeshBuilderTest() + : mBuilder(mSettings) + { + mSettings.mRecastScaleFactor = 1.0f; + mSettings.mTrianglesPerChunk = 256; + } + }; + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_bhv_triangle_mesh_shape) + { + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape shape(&mesh, true); + mBuilder.addObject(shape, btTransform::getIdentity()); + const auto recastMesh = mBuilder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 0, -1, + -1, 0, 1, + -1, 0, -1, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_transformed_bhv_triangle_mesh_shape) + { + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape shape(&mesh, true); + mBuilder.addObject(shape, + btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3))); + const auto recastMesh = mBuilder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 2, 3, 0, + 0, 3, 4, + 0, 3, 0, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_heightfield_terrian_shape) + { + const std::array heightfieldData {{0, 0, 0, 0}}; + btHeightfieldTerrainShape shape(2, 2, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + mBuilder.addObject(shape, btTransform::getIdentity()); + const auto recastMesh = mBuilder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + -0.5, 0, -0.5, + -0.5, 0, 0.5, + 0.5, 0, -0.5, + 0.5, 0, -0.5, + -0.5, 0, 0.5, + 0.5, 0, 0.5, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2, 3, 4, 5})); + } +} diff --git a/apps/openmw_test_suite/detournavigator/settingsutils.cpp b/apps/openmw_test_suite/detournavigator/settingsutils.cpp new file mode 100644 index 0000000000..ffed64ab81 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/settingsutils.cpp @@ -0,0 +1,68 @@ +#include "operators.hpp" + +#include + +#include + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorGetTilePositionTest : Test + { + Settings mSettings; + + DetourNavigatorGetTilePositionTest() + { + mSettings.mCellSize = 0.5; + mSettings.mTileSize = 64; + } + }; + + TEST_F(DetourNavigatorGetTilePositionTest, for_zero_coordinates_should_return_zero_tile_position) + { + EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(0, 0, 0)), TilePosition(0, 0)); + } + + TEST_F(DetourNavigatorGetTilePositionTest, tile_size_should_be_multiplied_by_cell_size) + { + EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(32, 0, 0)), TilePosition(1, 0)); + } + + TEST_F(DetourNavigatorGetTilePositionTest, tile_position_calculates_by_floor) + { + EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(31, 0, 0)), TilePosition(0, 0)); + } + + TEST_F(DetourNavigatorGetTilePositionTest, tile_position_depends_on_x_and_z_coordinates) + { + EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(32, 64, 128)), TilePosition(1, 4)); + } + + TEST_F(DetourNavigatorGetTilePositionTest, tile_position_works_for_negative_coordinates) + { + EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(-31, 0, -32)), TilePosition(-1, -1)); + } + + struct DetourNavigatorMakeTileBoundsTest : Test + { + Settings mSettings; + + DetourNavigatorMakeTileBoundsTest() + { + mSettings.mCellSize = 0.5; + mSettings.mTileSize = 64; + } + }; + + TEST_F(DetourNavigatorMakeTileBoundsTest, tile_bounds_depend_on_tile_size_and_cell_size) + { + EXPECT_EQ(makeTileBounds(mSettings, TilePosition(0, 0)), (TileBounds {osg::Vec2f(0, 0), osg::Vec2f(32, 32)})); + } + + TEST_F(DetourNavigatorMakeTileBoundsTest, tile_bounds_are_multiplied_by_tile_position) + { + EXPECT_EQ(makeTileBounds(mSettings, TilePosition(1, 2)), (TileBounds {osg::Vec2f(32, 64), osg::Vec2f(64, 96)})); + } +} diff --git a/components/CMakeLists.txt b/components/CMakeLists.txt index 5c245afd08..2b857d8111 100644 --- a/components/CMakeLists.txt +++ b/components/CMakeLists.txt @@ -156,6 +156,20 @@ if(NOT WIN32 AND NOT ANDROID) ) endif() +add_component_dir(detournavigator + debug + makenavmesh + findsmoothpath + recastmeshbuilder + recastmeshmanager + cachedrecastmeshmanager + navmeshmanager + navigator + asyncnavmeshupdater + chunkytrimesh + recastmesh + ) + set (ESM_UI ${CMAKE_SOURCE_DIR}/files/ui/contentselector.ui ) @@ -196,6 +210,10 @@ include_directories(${Bullet_INCLUDE_DIRS} ${CMAKE_CURRENT_BINARY_DIR}) add_library(components STATIC ${COMPONENT_FILES} ${MOC_SRCS} ${ESM_UI_HDR}) +find_package(RecastNavigation COMPONENTS Detour Recast REQUIRED) + +include_directories(SYSTEM ${RecastNavigation_INCLUDE_DIRS}) + target_link_libraries(components ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} @@ -214,6 +232,7 @@ target_link_libraries(components ${SDL2_LIBRARIES} ${OPENGL_gl_LIBRARY} ${MyGUI_LIBRARIES} + ${RecastNavigation_LIBRARIES} ) if (WIN32) diff --git a/components/detournavigator/asyncnavmeshupdater.cpp b/components/detournavigator/asyncnavmeshupdater.cpp new file mode 100644 index 0000000000..3a95f19d90 --- /dev/null +++ b/components/detournavigator/asyncnavmeshupdater.cpp @@ -0,0 +1,113 @@ +#include "asyncnavmeshupdater.hpp" +#include "debug.hpp" +#include "makenavmesh.hpp" +#include "settings.hpp" + +#include + +#include + +namespace DetourNavigator +{ + AsyncNavMeshUpdater::AsyncNavMeshUpdater(const Settings& settings) + : mSettings(settings) + , mMaxRevision(0) + , mShouldStop() + , mThread([&] { process(); }) + { + } + + AsyncNavMeshUpdater::~AsyncNavMeshUpdater() + { + mShouldStop = true; + std::unique_lock lock(mMutex); + mJobs.clear(); + mHasJob.notify_all(); + lock.unlock(); + mThread.join(); + } + + void AsyncNavMeshUpdater::post(const osg::Vec3f& agentHalfExtents, const std::shared_ptr& recastMesh, + const std::shared_ptr& navMeshCacheItem) + { + const std::lock_guard lock(mMutex); + mJobs[agentHalfExtents] = Job {agentHalfExtents, recastMesh, navMeshCacheItem}; + mHasJob.notify_all(); + } + + void AsyncNavMeshUpdater::wait() + { + std::unique_lock lock(mMutex); + mDone.wait(lock, [&] { return mJobs.empty(); }); + } + + void AsyncNavMeshUpdater::process() throw() + { + log("start process jobs"); + while (!mShouldStop) + { + try + { + if (const auto job = getNextJob()) + processJob(*job); + } + catch (const std::exception& e) + { + DetourNavigator::log("AsyncNavMeshUpdater::process exception: ", e.what()); + ::Log(Debug::Error) << "Exception while process navmesh updated job: " << e.what(); + } + } + log("stop process jobs"); + } + + void AsyncNavMeshUpdater::processJob(const Job& job) + { + log("process job for agent=", job.mAgentHalfExtents, + " revision=", job.mNavMeshCacheItem->mRevision, + " max_revision=", mMaxRevision); + + if (job.mNavMeshCacheItem->mRevision < mMaxRevision) + return; + + mMaxRevision = job.mNavMeshCacheItem->mRevision; + + const auto start = std::chrono::steady_clock::now(); + + job.mNavMeshCacheItem->mValue = makeNavMesh(job.mAgentHalfExtents, *job.mRecastMesh, mSettings); + + const auto finish = std::chrono::steady_clock::now(); + + writeDebugFiles(job); + + using FloatMs = std::chrono::duration; + + log("cache updated for agent=", job.mAgentHalfExtents, + " time=", std::chrono::duration_cast(finish - start).count(), "ms"); + } + + boost::optional AsyncNavMeshUpdater::getNextJob() + { + std::unique_lock lock(mMutex); + if (mJobs.empty()) + mHasJob.wait_for(lock, std::chrono::milliseconds(10)); + if (mJobs.empty()) + { + mDone.notify_all(); + return boost::none; + } + log("got ", mJobs.size(), " jobs"); + const auto job = mJobs.begin()->second; + mJobs.erase(mJobs.begin()); + return job; + } + + void AsyncNavMeshUpdater::writeDebugFiles(const Job& job) const + { +#ifdef OPENMW_WRITE_TO_FILE + const auto revision = std::to_string((std::chrono::steady_clock::now() + - std::chrono::steady_clock::time_point()).count()); + writeToFile(*job.mRecastMesh, revision); + writeToFile(*job.mNavMeshCacheItem->mValue, revision); +#endif + } +} diff --git a/components/detournavigator/asyncnavmeshupdater.hpp b/components/detournavigator/asyncnavmeshupdater.hpp new file mode 100644 index 0000000000..cdcc3432c9 --- /dev/null +++ b/components/detournavigator/asyncnavmeshupdater.hpp @@ -0,0 +1,75 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H + +#include "recastmesh.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + using NavMeshConstPtr = std::shared_ptr; + + struct NavMeshCacheItem + { + NavMeshConstPtr mValue = nullptr; + std::size_t mRevision; + + NavMeshCacheItem(std::size_t mRevision) + : mRevision(mRevision) + {} + }; + + class AsyncNavMeshUpdater + { + public: + AsyncNavMeshUpdater(const Settings& settings); + ~AsyncNavMeshUpdater(); + + void post(const osg::Vec3f& agentHalfExtents, const std::shared_ptr& recastMesh, + const std::shared_ptr& navMeshCacheItem); + + void wait(); + + private: + struct Job + { + osg::Vec3f mAgentHalfExtents; + std::shared_ptr mRecastMesh; + std::shared_ptr mNavMeshCacheItem; + }; + + using Jobs = std::map; + + std::reference_wrapper mSettings; + std::atomic_size_t mMaxRevision; + std::atomic_bool mShouldStop; + std::mutex mMutex; + std::condition_variable mHasJob; + std::condition_variable mDone; + Jobs mJobs; + std::thread mThread; + + void process() throw(); + + void processJob(const Job& job); + + boost::optional getNextJob(); + + void notifyHasJob(); + + void writeDebugFiles(const Job& job) const; + }; +} + +#endif diff --git a/components/detournavigator/cachedrecastmeshmanager.cpp b/components/detournavigator/cachedrecastmeshmanager.cpp new file mode 100644 index 0000000000..1267320380 --- /dev/null +++ b/components/detournavigator/cachedrecastmeshmanager.cpp @@ -0,0 +1,25 @@ +#include "cachedrecastmeshmanager.hpp" +#include "debug.hpp" + +namespace DetourNavigator +{ + CachedRecastMeshManager::CachedRecastMeshManager(const Settings& settings) + : mImpl(settings) + { + } + + bool CachedRecastMeshManager::removeObject(std::size_t id) + { + if (!mImpl.removeObject(id)) + return false; + mCached.reset(); + return true; + } + + std::shared_ptr CachedRecastMeshManager::getMesh() + { + if (!mCached) + mCached = mImpl.getMesh(); + return mCached; + } +} diff --git a/components/detournavigator/cachedrecastmeshmanager.hpp b/components/detournavigator/cachedrecastmeshmanager.hpp new file mode 100644 index 0000000000..3cfe9a56f5 --- /dev/null +++ b/components/detournavigator/cachedrecastmeshmanager.hpp @@ -0,0 +1,34 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_CACHEDRECASTMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_CACHEDRECASTMESHMANAGER_H + +#include "recastmeshmanager.hpp" + +#include + +namespace DetourNavigator +{ + class CachedRecastMeshManager + { + public: + CachedRecastMeshManager(const Settings& settings); + + template + bool addObject(std::size_t id, const T& shape, const btTransform& transform) + { + if (!mImpl.addObject(id, shape, transform)) + return false; + mCached.reset(); + return true; + } + + bool removeObject(std::size_t id); + + std::shared_ptr getMesh(); + + private: + RecastMeshManager mImpl; + std::shared_ptr mCached; + }; +} + +#endif diff --git a/components/detournavigator/chunkytrimesh.cpp b/components/detournavigator/chunkytrimesh.cpp new file mode 100644 index 0000000000..9402569fc1 --- /dev/null +++ b/components/detournavigator/chunkytrimesh.cpp @@ -0,0 +1,174 @@ +#include "chunkytrimesh.hpp" + +#include + +#include + +namespace DetourNavigator +{ + namespace + { + struct BoundsItem + { + Rect mBounds; + std::ptrdiff_t mOffset; + }; + + template + struct LessBoundsItem + { + bool operator ()(const BoundsItem& lhs, const BoundsItem& rhs) const + { + return lhs.mBounds.mMinBound[axis] < rhs.mBounds.mMinBound[axis]; + } + }; + + void calcExtends(const std::vector& items, const std::size_t imin, const std::size_t imax, + Rect& bounds) + { + bounds = items[imin].mBounds; + + std::for_each( + items.begin() + static_cast(imin) + 1, + items.begin() + static_cast(imax), + [&] (const BoundsItem& item) + { + for (int i = 0; i < 2; ++i) + { + bounds.mMinBound[i] = std::min(bounds.mMinBound[i], item.mBounds.mMinBound[i]); + bounds.mMaxBound[i] = std::max(bounds.mMaxBound[i], item.mBounds.mMaxBound[i]); + } + }); + } + + void subdivide(std::vector& items, const std::size_t imin, const std::size_t imax, + const std::size_t trisPerChunk, const std::vector& inIndices, + std::size_t& curNode, std::vector& nodes, std::size_t& curTri, + std::vector& outIndices) + { + const auto inum = imax - imin; + const auto icur = curNode; + + if (curNode > nodes.size()) + return; + + ChunkyTriMeshNode& node = nodes[curNode++]; + + if (inum <= trisPerChunk) + { + // Leaf + calcExtends(items, imin, imax, node.mBounds); + + // Copy triangles. + node.mOffset = static_cast(curTri); + node.mSize = inum; + + for (std::size_t i = imin; i < imax; ++i) + { + std::copy( + inIndices.begin() + items[i].mOffset * 3, + inIndices.begin() + items[i].mOffset * 3 + 3, + outIndices.begin() + static_cast(curTri) * 3 + ); + curTri++; + } + } + else + { + // Split + calcExtends(items, imin, imax, node.mBounds); + + if (node.mBounds.mMaxBound.x() - node.mBounds.mMinBound.x() + >= node.mBounds.mMaxBound.y() - node.mBounds.mMinBound.y()) + { + // Sort along x-axis + std::sort( + items.begin() + static_cast(imin), + items.begin() + static_cast(imax), + LessBoundsItem<0> {} + ); + } + else + { + // Sort along y-axis + std::sort( + items.begin() + static_cast(imin), + items.begin() + static_cast(imax), + LessBoundsItem<1> {} + ); + } + + const auto isplit = imin + inum / 2; + + // Left + subdivide(items, imin, isplit, trisPerChunk, inIndices, curNode, nodes, curTri, outIndices); + // Right + subdivide(items, isplit, imax, trisPerChunk, inIndices, curNode, nodes, curTri, outIndices); + + const auto iescape = static_cast(curNode) - static_cast(icur); + // Negative index means escape. + node.mOffset = -iescape; + } + } + } + + ChunkyTriMesh::ChunkyTriMesh(const std::vector& verts, const std::vector& indices, + const std::size_t trisPerChunk) + : mMaxTrisPerChunk(0) + { + const auto trianglesCount = indices.size() / 3; + + if (trianglesCount == 0) + return; + + const auto nchunks = (trianglesCount + trisPerChunk - 1) / trisPerChunk; + + mNodes.resize(nchunks * 4); + mIndices.resize(trianglesCount * 3); + + // Build tree + std::vector items(trianglesCount); + + for (std::size_t i = 0; i < trianglesCount; i++) + { + auto& item = items[i]; + + item.mOffset = static_cast(i); + + // Calc triangle XZ bounds. + const auto baseIndex = static_cast(indices[i * 3]) * 3; + + item.mBounds.mMinBound.x() = item.mBounds.mMaxBound.x() = verts[baseIndex + 0]; + item.mBounds.mMinBound.y() = item.mBounds.mMaxBound.y() = verts[baseIndex + 2]; + + for (std::size_t j = 1; j < 3; ++j) + { + const auto index = static_cast(indices[i * 3 + j]) * 3; + + item.mBounds.mMinBound.x() = std::min(item.mBounds.mMinBound.x(), verts[index + 0]); + item.mBounds.mMinBound.y() = std::min(item.mBounds.mMinBound.y(), verts[index + 2]); + + item.mBounds.mMaxBound.x() = std::max(item.mBounds.mMaxBound.x(), verts[index + 0]); + item.mBounds.mMaxBound.y() = std::max(item.mBounds.mMaxBound.y(), verts[index + 2]); + } + } + + std::size_t curTri = 0; + std::size_t curNode = 0; + subdivide(items, 0, trianglesCount, trisPerChunk, indices, curNode, mNodes, curTri, mIndices); + + items.clear(); + + mNodes.resize(curNode); + + // Calc max tris per node. + for (auto& node : mNodes) + { + const bool isLeaf = node.mOffset >= 0; + if (!isLeaf) + continue; + if (node.mSize > mMaxTrisPerChunk) + mMaxTrisPerChunk = node.mSize; + } + } +} diff --git a/components/detournavigator/chunkytrimesh.hpp b/components/detournavigator/chunkytrimesh.hpp new file mode 100644 index 0000000000..bc1898e2fe --- /dev/null +++ b/components/detournavigator/chunkytrimesh.hpp @@ -0,0 +1,93 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_CHUNKYTRIMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_CHUNKYTRIMESH_H + +#include + +#include +#include + +namespace DetourNavigator +{ + struct Rect + { + osg::Vec2f mMinBound; + osg::Vec2f mMaxBound; + }; + + struct ChunkyTriMeshNode + { + Rect mBounds; + std::ptrdiff_t mOffset; + std::size_t mSize; + }; + + struct Chunk + { + const int* const mIndices; + const std::size_t mSize; + }; + + inline bool checkOverlapRect(const Rect& lhs, const Rect& rhs) + { + bool overlap = true; + overlap = (lhs.mMinBound.x() > rhs.mMaxBound.x() || lhs.mMaxBound.x() < rhs.mMinBound.x()) ? false : overlap; + overlap = (lhs.mMinBound.y() > rhs.mMaxBound.y() || lhs.mMaxBound.y() < rhs.mMinBound.y()) ? false : overlap; + return overlap; + } + + class ChunkyTriMesh + { + public: + /// Creates partitioned triangle mesh (AABB tree), + /// where each node contains at max trisPerChunk triangles. + ChunkyTriMesh(const std::vector& verts, const std::vector& tris, std::size_t trisPerChunk); + + ChunkyTriMesh(const ChunkyTriMesh&) = delete; + ChunkyTriMesh& operator=(const ChunkyTriMesh&) = delete; + + /// Returns the chunk indices which overlap the input rectable. + template + void getChunksOverlappingRect(const Rect& rect, OutputIterator out) const + { + // Traverse tree + for (std::size_t i = 0; i < mNodes.size(); ) + { + const ChunkyTriMeshNode* node = &mNodes[i]; + const bool overlap = checkOverlapRect(rect, node->mBounds); + const bool isLeafNode = node->mOffset >= 0; + + if (isLeafNode && overlap) + *out++ = i; + + if (overlap || isLeafNode) + i++; + else + { + const auto escapeIndex = -node->mOffset; + i += static_cast(escapeIndex); + } + } + } + + Chunk getChunk(const std::size_t chunkId) const + { + const auto& node = mNodes[chunkId]; + return Chunk { + mIndices.data() + node.mOffset * 3, + node.mSize + }; + } + + std::size_t getMaxTrisPerChunk() const + { + return mMaxTrisPerChunk; + } + + private: + std::vector mNodes; + std::vector mIndices; + std::size_t mMaxTrisPerChunk; + }; +} + +#endif diff --git a/components/detournavigator/debug.cpp b/components/detournavigator/debug.cpp new file mode 100644 index 0000000000..0b783fd908 --- /dev/null +++ b/components/detournavigator/debug.cpp @@ -0,0 +1,163 @@ +#include "debug.hpp" + +#define OPENMW_WRITE_TO_FILE +#define OPENMW_WRITE_OBJ + +#ifdef OPENMW_WRITE_OBJ +#include "exceptions.hpp" + +#include +#endif + +#ifdef OPENMW_WRITE_TO_FILE +#include "exceptions.hpp" +#include "recastmesh.hpp" + +#include + +#include +#endif + +#include +#include + +namespace +{ +#ifdef OPENMW_WRITE_TO_FILE + static const int NAVMESHSET_MAGIC = 'M' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'MSET'; + static const int NAVMESHSET_VERSION = 1; + + struct NavMeshSetHeader + { + int magic; + int version; + int numTiles; + dtNavMeshParams params; + }; + + struct NavMeshTileHeader + { + dtTileRef tileRef; + int dataSize; + }; +#endif +} + +namespace DetourNavigator +{ +// Use to dump scene to load from recastnavigation demo tool +#ifdef OPENMW_WRITE_OBJ + void writeObj(const std::vector& vertices, const std::vector& indices) + { + const auto path = std::string("scene.") + std::to_string(std::time(nullptr)) + ".obj"; + std::ofstream file(path); + if (!file.is_open()) + throw NavigatorException("Open file failed: " + path); + file.exceptions(std::ios::failbit | std::ios::badbit); + file.precision(std::numeric_limits::max_exponent10); + std::size_t count = 0; + for (auto v : vertices) + { + if (count % 3 == 0) + { + if (count != 0) + file << '\n'; + file << 'v'; + } + file << ' ' << v; + ++count; + } + file << '\n'; + count = 0; + for (auto v : indices) + { + if (count % 3 == 0) + { + if (count != 0) + file << '\n'; + file << 'f'; + } + file << ' ' << (v + 1); + ++count; + } + file << '\n'; + } +#endif + +#ifdef OPENMW_WRITE_TO_FILE + void writeToFile(const RecastMesh& recastMesh, const std::string& revision) + { + const auto path = "recastmesh." + revision + ".obj"; + std::ofstream file(path); + if (!file.is_open()) + throw NavigatorException("Open file failed: " + path); + file.exceptions(std::ios::failbit | std::ios::badbit); + file.precision(std::numeric_limits::max_exponent10); + std::size_t count = 0; + for (auto v : recastMesh.getVertices()) + { + if (count % 3 == 0) + { + if (count != 0) + file << '\n'; + file << 'v'; + } + file << ' ' << v; + ++count; + } + file << '\n'; + count = 0; + for (auto v : recastMesh.getIndices()) + { + if (count % 3 == 0) + { + if (count != 0) + file << '\n'; + file << 'f'; + } + file << ' ' << (v + 1); + ++count; + } + file << '\n'; + } + + void writeToFile(const dtNavMesh& navMesh, const std::string& revision) + { + const auto path = "navmesh." + revision + ".bin"; + std::ofstream file(path, std::ios::binary); + if (!file.is_open()) + throw NavigatorException("Open file failed: " + path); + file.exceptions(std::ios::failbit | std::ios::badbit); + + NavMeshSetHeader header; + header.magic = NAVMESHSET_MAGIC; + header.version = NAVMESHSET_VERSION; + header.numTiles = 0; + for (int i = 0; i < navMesh.getMaxTiles(); ++i) + { + const auto tile = navMesh.getTile(i); + if (!tile || !tile->header || !tile->dataSize) + continue; + header.numTiles++; + } + header.params = *navMesh.getParams(); + + using const_char_ptr = const char*; + file.write(const_char_ptr(&header), sizeof(header)); + + for (int i = 0; i < navMesh.getMaxTiles(); ++i) + { + const auto tile = navMesh.getTile(i); + if (!tile || !tile->header || !tile->dataSize) + continue; + + NavMeshTileHeader tileHeader; + tileHeader.tileRef = navMesh.getTileRef(tile); + tileHeader.dataSize = tile->dataSize; + + file.write(const_char_ptr(&tileHeader), sizeof(tileHeader)); + file.write(const_char_ptr(tile->data), tile->dataSize); + } + } +#endif +} diff --git a/components/detournavigator/debug.hpp b/components/detournavigator/debug.hpp new file mode 100644 index 0000000000..c9f25ca496 --- /dev/null +++ b/components/detournavigator/debug.hpp @@ -0,0 +1,63 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H + +#include "tilebounds.hpp" + +#include +#include + +#include +#include +#include +#include + +#ifdef OPENMW_WRITE_OBJ +#include +#endif + +#ifdef OPENMW_WRITE_TO_FILE +class dtNavMesh; +#endif + +namespace DetourNavigator +{ + inline std::ostream& operator <<(std::ostream& stream, const TileBounds& value) + { + return stream << "TileBounds {" << value.mMin << ", " << value.mMax << "}"; + } + +// Use to dump scene to load from recastnavigation demo tool +#ifdef OPENMW_WRITE_OBJ + void writeObj(const std::vector& vertices, const std::vector& indices); +#endif + +#ifdef OPENMW_WRITE_TO_FILE + class RecastMesh; + + void writeToFile(const RecastMesh& recastMesh, const std::string& revision); + + void writeToFile(const dtNavMesh& navMesh, const std::string& revision); +#endif + + inline void write(std::ostream& stream) + { + stream << '\n'; + } + + template + void write(std::ostream& stream, const Head& head, const Tail& ... tail) + { + stream << head; + write(stream, tail ...); + } + + template + void log(Ts&& ... values) + { + std::ostringstream stream; + write(stream, std::forward(values) ...); + std::cout << stream.str(); + } +} + +#endif diff --git a/components/detournavigator/dtstatus.hpp b/components/detournavigator/dtstatus.hpp new file mode 100644 index 0000000000..5360b5ce36 --- /dev/null +++ b/components/detournavigator/dtstatus.hpp @@ -0,0 +1,66 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H + +#include "exceptions.hpp" + +#include + +#include +#include + +namespace DetourNavigator +{ + struct WriteDtStatus + { + dtStatus status; + }; + + static const std::vector> dtStatuses { + {DT_FAILURE, "DT_FAILURE"}, + {DT_SUCCESS, "DT_SUCCESS"}, + {DT_IN_PROGRESS, "DT_IN_PROGRESS"}, + {DT_WRONG_MAGIC, "DT_WRONG_MAGIC"}, + {DT_WRONG_VERSION, "DT_WRONG_VERSION"}, + {DT_OUT_OF_MEMORY, "DT_OUT_OF_MEMORY"}, + {DT_INVALID_PARAM, "DT_INVALID_PARAM"}, + {DT_BUFFER_TOO_SMALL, "DT_BUFFER_TOO_SMALL"}, + {DT_OUT_OF_NODES, "DT_OUT_OF_NODES"}, + {DT_PARTIAL_RESULT, "DT_PARTIAL_RESULT"}, + }; + + inline std::ostream& operator <<(std::ostream& stream, const WriteDtStatus& value) + { + for (const auto& status : dtStatuses) + if (value.status & status.first) + stream << status.second << " "; + return stream; + } + + inline void checkDtStatus(dtStatus status, const char* call, int line) + { + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << call << " failed with status=" << WriteDtStatus {status} << " at " __FILE__ ":" << line; + throw NavigatorException(message.str()); + } + } + + inline void checkDtResult(bool result, const char* call, int line) + { + if (!result) + { + std::ostringstream message; + message << call << " failed at " __FILE__ ":" << line; + throw NavigatorException(message.str()); + } + } +} + +#define OPENMW_CHECK_DT_STATUS(call) \ + do { DetourNavigator::checkDtStatus((call), #call, __LINE__); } while (false) + +#define OPENMW_CHECK_DT_RESULT(call) \ + do { DetourNavigator::checkDtResult((call), #call, __LINE__); } while (false) + +#endif diff --git a/components/detournavigator/exceptions.hpp b/components/detournavigator/exceptions.hpp new file mode 100644 index 0000000000..e547aaaf4d --- /dev/null +++ b/components/detournavigator/exceptions.hpp @@ -0,0 +1,15 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_EXCEPTIONS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_EXCEPTIONS_H + +#include + +namespace DetourNavigator +{ + struct NavigatorException : std::runtime_error + { + NavigatorException(const std::string& message) : std::runtime_error(message) {} + NavigatorException(const char* message) : std::runtime_error(message) {} + }; +} + +#endif diff --git a/components/detournavigator/findsmoothpath.cpp b/components/detournavigator/findsmoothpath.cpp new file mode 100644 index 0000000000..e59b801144 --- /dev/null +++ b/components/detournavigator/findsmoothpath.cpp @@ -0,0 +1,143 @@ +#include "findsmoothpath.hpp" + +#include +#include + +namespace DetourNavigator +{ + std::vector fixupCorridor(const std::vector& path, const std::vector& visited) + { + std::vector::const_reverse_iterator furthestVisited; + + // Find furthest common polygon. + const auto it = std::find_if(path.rbegin(), path.rend(), [&] (dtPolyRef pathValue) + { + const auto it = std::find(visited.rbegin(), visited.rend(), pathValue); + if (it == visited.rend()) + return false; + furthestVisited = it; + return true; + }); + + // If no intersection found just return current path. + if (it == path.rend()) + return path; + const auto furthestPath = it.base() - 1; + + // Concatenate paths. + + // visited: a_1 ... a_n x b_1 ... b_n + // furthestVisited ^ + // path: C x D + // ^ furthestPath + // result: x b_n ... b_1 D + + std::vector result; + result.reserve(static_cast(furthestVisited - visited.rbegin()) + + static_cast(path.end() - furthestPath) - 1); + std::copy(visited.rbegin(), furthestVisited + 1, std::back_inserter(result)); + std::copy(furthestPath + 1, path.end(), std::back_inserter(result)); + + return result; + } + + // This function checks if the path has a small U-turn, that is, + // a polygon further in the path is adjacent to the first polygon + // in the path. If that happens, a shortcut is taken. + // This can happen if the target (T) location is at tile boundary, + // and we're (S) approaching it parallel to the tile edge. + // The choice at the vertex can be arbitrary, + // +---+---+ + // |:::|:::| + // +-S-+-T-+ + // |:::| | <-- the step can end up in here, resulting U-turn path. + // +---+---+ + std::vector fixupShortcuts(const std::vector& path, const dtNavMeshQuery& navQuery) + { + if (path.size() < 3) + return path; + + // Get connected polygons + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(navQuery.getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly))) + return path; + + const std::size_t maxNeis = 16; + std::array neis; + std::size_t nneis = 0; + + for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next) + { + const dtLink* link = &tile->links[k]; + if (link->ref != 0) + { + if (nneis < maxNeis) + neis[nneis++] = link->ref; + } + } + + // If any of the neighbour polygons is within the next few polygons + // in the path, short cut to that polygon directly. + const std::size_t maxLookAhead = 6; + std::size_t cut = 0; + for (std::size_t i = std::min(maxLookAhead, path.size()) - 1; i > 1 && cut == 0; i--) + { + for (std::size_t j = 0; j < nneis; j++) + { + if (path[i] == neis[j]) + { + cut = i; + break; + } + } + } + if (cut <= 1) + return path; + + std::vector result; + const auto offset = cut - 1; + result.reserve(1 + path.size() - offset); + result.push_back(path.front()); + std::copy(path.begin() + std::ptrdiff_t(offset), path.end(), std::back_inserter(result)); + return result; + } + + boost::optional getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos, + const osg::Vec3f& endPos, const float minTargetDist, const std::vector& path) + { + // Find steer target. + SteerTarget result; + const int MAX_STEER_POINTS = 3; + std::array steerPath; + std::array steerPathFlags; + std::array steerPathPolys; + int nsteerPath = 0; + navQuery.findStraightPath(startPos.ptr(), endPos.ptr(), path.data(), int(path.size()), steerPath.data(), + steerPathFlags.data(), steerPathPolys.data(), &nsteerPath, MAX_STEER_POINTS); + assert(nsteerPath >= 0); + if (!nsteerPath) + return boost::none; + + // Find vertex far enough to steer to. + std::size_t ns = 0; + while (ns < static_cast(nsteerPath)) + { + // Stop at Off-Mesh link or when point is further than slop away. + if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) || + !inRange(makeOsgVec3f(&steerPath[ns * 3]), startPos, minTargetDist, 1000.0f)) + break; + ns++; + } + // Failed to find good point to steer to. + if (ns >= static_cast(nsteerPath)) + return boost::none; + + dtVcopy(result.steerPos.ptr(), &steerPath[ns * 3]); + result.steerPos.y() = startPos[1]; + result.steerPosFlag = steerPathFlags[ns]; + result.steerPosRef = steerPathPolys[ns]; + + return result; + } +} diff --git a/components/detournavigator/findsmoothpath.hpp b/components/detournavigator/findsmoothpath.hpp new file mode 100644 index 0000000000..2fbc07c5fd --- /dev/null +++ b/components/detournavigator/findsmoothpath.hpp @@ -0,0 +1,266 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H + +#include "dtstatus.hpp" +#include "exceptions.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" + +#include +#include +#include + +#include + +#include + +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + struct Settings; + + inline osg::Vec3f makeOsgVec3f(const float* values) + { + return osg::Vec3f(values[0], values[1], values[2]); + } + + inline bool inRange(const osg::Vec3f& v1, const osg::Vec3f& v2, const float r, const float h) + { + const auto d = v2 - v1; + return (d.x() * d.x() + d.z() * d.z()) < r * r && std::abs(d.y()) < h; + } + + std::vector fixupCorridor(const std::vector& path, const std::vector& visited); + + // This function checks if the path has a small U-turn, that is, + // a polygon further in the path is adjacent to the first polygon + // in the path. If that happens, a shortcut is taken. + // This can happen if the target (T) location is at tile boundary, + // and we're (S) approaching it parallel to the tile edge. + // The choice at the vertex can be arbitrary, + // +---+---+ + // |:::|:::| + // +-S-+-T-+ + // |:::| | <-- the step can end up in here, resulting U-turn path. + // +---+---+ + std::vector fixupShortcuts(const std::vector& path, const dtNavMeshQuery& navQuery); + + struct SteerTarget + { + osg::Vec3f steerPos; + unsigned char steerPosFlag; + dtPolyRef steerPosRef; + }; + + boost::optional getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos, + const osg::Vec3f& endPos, const float minTargetDist, const std::vector& path); + + template + class OutputTransformIterator + { + public: + OutputTransformIterator(OutputIterator& impl, const Settings& settings) + : mImpl(impl), mSettings(settings) + { + } + + OutputTransformIterator& operator *() + { + return *this; + } + + OutputTransformIterator& operator ++(int) + { + mImpl++; + return *this; + } + + OutputTransformIterator& operator =(const osg::Vec3f& value) + { + *mImpl = fromNavMeshCoordinates(mSettings, value); + return *this; + } + + private: + OutputIterator& mImpl; + const Settings& mSettings; + }; + + template + OutputIterator makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, + const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end, + std::vector polygonPath, std::size_t maxSmoothPathSize, OutputIterator out) + { + // Iterate over the path to find smooth path on the detail mesh surface. + osg::Vec3f iterPos; + navMeshQuery.closestPointOnPoly(polygonPath.front(), start.ptr(), iterPos.ptr(), 0); + + osg::Vec3f targetPos; + navMeshQuery.closestPointOnPoly(polygonPath.back(), end.ptr(), targetPos.ptr(), 0); + + const float STEP_SIZE = 0.5f; + const float SLOP = 0.01f; + + *out++ = iterPos; + + std::size_t smoothPathSize = 1; + + // Move towards target a small advancement at a time until target reached or + // when ran out of memory to store the path. + while (!polygonPath.empty() && smoothPathSize < maxSmoothPathSize) + { + // Find location to steer towards. + const auto steerTarget = getSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, polygonPath); + + if (!steerTarget) + break; + + const bool endOfPath = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_END); + const bool offMeshConnection = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION); + + // Find movement delta. + const osg::Vec3f delta = steerTarget->steerPos - iterPos; + float len = delta.length(); + // If the steer target is end of path or off-mesh link, do not move past the location. + if ((endOfPath || offMeshConnection) && len < STEP_SIZE) + len = 1; + else + len = STEP_SIZE / len; + + const osg::Vec3f moveTgt = iterPos + delta * len; + + // Move + osg::Vec3f result; + std::vector visited(16); + int nvisited = 0; + OPENMW_CHECK_DT_STATUS(navMeshQuery.moveAlongSurface(polygonPath.front(), iterPos.ptr(), moveTgt.ptr(), + &filter, result.ptr(), visited.data(), &nvisited, int(visited.size()))); + + assert(nvisited >= 0); + assert(nvisited <= int(visited.size())); + visited.resize(static_cast(nvisited)); + + polygonPath = fixupCorridor(polygonPath, visited); + polygonPath = fixupShortcuts(polygonPath, navMeshQuery); + + float h = 0; + navMeshQuery.getPolyHeight(polygonPath.front(), result.ptr(), &h); + result.y() = h; + iterPos = result; + + // Handle end of path and off-mesh links when close enough. + if (endOfPath && inRange(iterPos, steerTarget->steerPos, SLOP, 1.0f)) + { + // Reached end of path. + iterPos = targetPos; + *out++ = iterPos; + ++smoothPathSize; + break; + } + else if (offMeshConnection && inRange(iterPos, steerTarget->steerPos, SLOP, 1.0f)) + { + // Advance the path up to and over the off-mesh connection. + dtPolyRef prevRef = 0; + dtPolyRef polyRef = polygonPath.front(); + std::size_t npos = 0; + while (npos < polygonPath.size() && polyRef != steerTarget->steerPosRef) + { + prevRef = polyRef; + polyRef = polygonPath[npos]; + ++npos; + } + std::copy(polygonPath.begin() + std::ptrdiff_t(npos), polygonPath.end(), polygonPath.begin()); + polygonPath.resize(polygonPath.size() - npos); + + // Reached off-mesh connection. + osg::Vec3f startPos; + osg::Vec3f endPos; + + // Handle the connection. + if (dtStatusSucceed(navMesh.getOffMeshConnectionPolyEndPoints(prevRef, polyRef, + startPos.ptr(), endPos.ptr()))) + { + *out++ = startPos; + ++smoothPathSize; + + // Hack to make the dotted path not visible during off-mesh connection. + if (smoothPathSize & 1) + { + *out++ = startPos; + ++smoothPathSize; + } + + // Move position at the other side of the off-mesh link. + iterPos = endPos; + float eh = 0.0f; + OPENMW_CHECK_DT_STATUS(navMeshQuery.getPolyHeight(polygonPath.front(), iterPos.ptr(), &eh)); + iterPos.y() = eh; + } + } + + // Store results. + *out++ = iterPos; + ++smoothPathSize; + } + + return out; + } + + template + OutputIterator findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, + const osg::Vec3f& start, const osg::Vec3f& end, const Settings& settings, OutputIterator out) + { + dtNavMeshQuery navMeshQuery; + OPENMW_CHECK_DT_STATUS(navMeshQuery.init(&navMesh, settings.mMaxNavMeshQueryNodes)); + + dtQueryFilter queryFilter; + + dtPolyRef startRef = 0; + osg::Vec3f startPolygonPosition; + for (int i = 0; i < 3; ++i) + { + const auto status = navMeshQuery.findNearestPoly(start.ptr(), (halfExtents * (1 << i)).ptr(), &queryFilter, + &startRef, startPolygonPosition.ptr()); + if (!dtStatusFailed(status) && startRef != 0) + break; + } + + if (startRef == 0) + throw NavigatorException("start polygon is not found at " __FILE__ ":" + std::to_string(__LINE__)); + + dtPolyRef endRef = 0; + osg::Vec3f endPolygonPosition; + for (int i = 0; i < 3; ++i) + { + const auto status = navMeshQuery.findNearestPoly(end.ptr(), (halfExtents * (1 << i)).ptr(), &queryFilter, + &endRef, endPolygonPosition.ptr()); + if (!dtStatusFailed(status) && endRef != 0) + break; + } + + if (endRef == 0) + throw NavigatorException("end polygon is not found at " __FILE__ ":" + std::to_string(__LINE__)); + + std::vector polygonPath(settings.mMaxPolygonPathSize); + int pathLen = 0; + OPENMW_CHECK_DT_STATUS(navMeshQuery.findPath(startRef, endRef, start.ptr(), end.ptr(), &queryFilter, + polygonPath.data(), &pathLen, static_cast(polygonPath.size()))); + + assert(pathLen >= 0); + + polygonPath.resize(static_cast(pathLen)); + + if (polygonPath.empty() || polygonPath.back() != endRef) + return out; + + makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, std::move(polygonPath), + settings.mMaxSmoothPathSize, OutputTransformIterator(out, settings)); + + return out; + } +} + +#endif diff --git a/components/detournavigator/makenavmesh.cpp b/components/detournavigator/makenavmesh.cpp new file mode 100644 index 0000000000..ef9b063747 --- /dev/null +++ b/components/detournavigator/makenavmesh.cpp @@ -0,0 +1,288 @@ +#include "makenavmesh.hpp" +#include "chunkytrimesh.hpp" +#include "dtstatus.hpp" +#include "exceptions.hpp" +#include "recastmesh.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" + +#include +#include +#include +#include + +#include + +namespace +{ + using namespace DetourNavigator; + + void initPolyMeshDetail(rcPolyMeshDetail& value) + { + value.meshes = nullptr; + value.verts = nullptr; + value.tris = nullptr; + } + + struct PolyMeshDetailStackDeleter + { + void operator ()(rcPolyMeshDetail* value) const + { + rcFree(value->meshes); + rcFree(value->verts); + rcFree(value->tris); + } + }; + + using PolyMeshDetailStackPtr = std::unique_ptr; + + struct NavMeshDataValueDeleter + { + void operator ()(unsigned char* value) const + { + dtFree(value); + } + }; + + using NavMeshDataValue = std::unique_ptr; + + struct NavMeshData + { + NavMeshDataValue mValue; + int mSize; + + NavMeshData() = default; + + NavMeshData(unsigned char* value, int size) + : mValue(value) + , mSize(size) + {} + }; + + NavMeshData makeNavMeshTileData(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, + const int tileX, const int tileY, const osg::Vec3f& boundsMin, const osg::Vec3f& boundsMax, + const Settings& settings) + { + rcContext context; + rcConfig config; + + config.cs = settings.mCellSize; + config.ch = settings.mCellHeight; + config.walkableSlopeAngle = settings.mMaxSlope; + config.walkableHeight = static_cast(std::ceil(getHeight(settings, agentHalfExtents) / config.ch)); + config.walkableClimb = static_cast(std::floor(getMaxClimb(settings) / config.ch)); + config.walkableRadius = static_cast(std::ceil(getRadius(settings, agentHalfExtents) / config.cs)); + config.maxEdgeLen = static_cast(std::round(settings.mMaxEdgeLen / config.cs)); + config.maxSimplificationError = settings.mMaxSimplificationError; + config.minRegionArea = settings.mRegionMinSize * settings.mRegionMinSize; + config.mergeRegionArea = settings.mRegionMergeSize * settings.mRegionMergeSize; + config.maxVertsPerPoly = settings.mMaxVertsPerPoly; + config.detailSampleDist = settings.mDetailSampleDist < 0.9f ? 0 : config.cs * settings.mDetailSampleDist; + config.detailSampleMaxError = config.ch * settings.mDetailSampleMaxError; + config.borderSize = config.walkableRadius + 3; + config.width = settings.mTileSize + config.borderSize * 2; + config.height = settings.mTileSize + config.borderSize * 2; + rcVcopy(config.bmin, boundsMin.ptr()); + rcVcopy(config.bmax, boundsMax.ptr()); + config.bmin[0] -= config.borderSize * config.cs; + config.bmin[2] -= config.borderSize * config.cs; + config.bmax[0] += config.borderSize * config.cs; + config.bmax[2] += config.borderSize * config.cs; + + rcHeightfield solid; + OPENMW_CHECK_DT_RESULT(rcCreateHeightfield(nullptr, solid, config.width, config.height, + config.bmin, config.bmax, config.cs, config.ch)); + + { + const auto& chunkyMesh = recastMesh.getChunkyTriMesh(); + std::vector areas(chunkyMesh.getMaxTrisPerChunk(), 0); + const osg::Vec2f tileBoundsMin(config.bmin[0], config.bmin[2]); + const osg::Vec2f tileBoundsMax(config.bmax[0], config.bmax[2]); + std::vector cids; + chunkyMesh.getChunksOverlappingRect(Rect {tileBoundsMin, tileBoundsMax}, std::back_inserter(cids)); + + if (cids.empty()) + return NavMeshData(); + + for (const auto cid : cids) + { + const auto chunk = chunkyMesh.getChunk(cid); + + std::fill( + areas.begin(), + std::min(areas.begin() + static_cast(chunk.mSize), + areas.end()), + 0 + ); + + rcMarkWalkableTriangles( + &context, + config.walkableSlopeAngle, + recastMesh.getVertices().data(), + static_cast(recastMesh.getVerticesCount()), + chunk.mIndices, + static_cast(chunk.mSize), + areas.data() + ); + + OPENMW_CHECK_DT_RESULT(rcRasterizeTriangles( + &context, + recastMesh.getVertices().data(), + static_cast(recastMesh.getVerticesCount()), + chunk.mIndices, + areas.data(), + static_cast(chunk.mSize), + solid, + config.walkableClimb + )); + } + } + + rcFilterLowHangingWalkableObstacles(&context, config.walkableClimb, solid); + rcFilterLedgeSpans(&context, config.walkableHeight, config.walkableClimb, solid); + rcFilterWalkableLowHeightSpans(&context, config.walkableHeight, solid); + + rcPolyMesh polyMesh; + rcPolyMeshDetail polyMeshDetail; + initPolyMeshDetail(polyMeshDetail); + const PolyMeshDetailStackPtr polyMeshDetailPtr(&polyMeshDetail); + { + rcCompactHeightfield compact; + + OPENMW_CHECK_DT_RESULT(rcBuildCompactHeightfield(&context, config.walkableHeight, config.walkableClimb, + solid, compact)); + OPENMW_CHECK_DT_RESULT(rcErodeWalkableArea(&context, config.walkableRadius, compact)); + OPENMW_CHECK_DT_RESULT(rcBuildDistanceField(&context, compact)); + OPENMW_CHECK_DT_RESULT(rcBuildRegions(&context, compact, config.borderSize, config.minRegionArea, + config.mergeRegionArea)); + + rcContourSet contourSet; + OPENMW_CHECK_DT_RESULT(rcBuildContours(&context, compact, config.maxSimplificationError, config.maxEdgeLen, + contourSet)); + + if (contourSet.nconts == 0) + return NavMeshData(); + + OPENMW_CHECK_DT_RESULT(rcBuildPolyMesh(&context, contourSet, config.maxVertsPerPoly, polyMesh)); + OPENMW_CHECK_DT_RESULT(rcBuildPolyMeshDetail(&context, polyMesh, compact, config.detailSampleDist, + config.detailSampleMaxError, polyMeshDetail)); + } + + for (int i = 0; i < polyMesh.npolys; ++i) + if (polyMesh.areas[i] == RC_WALKABLE_AREA) + polyMesh.flags[i] = 1; + + dtNavMeshCreateParams params; + params.verts = polyMesh.verts; + params.vertCount = polyMesh.nverts; + params.polys = polyMesh.polys; + params.polyAreas = polyMesh.areas; + params.polyFlags = polyMesh.flags; + params.polyCount = polyMesh.npolys; + params.nvp = polyMesh.nvp; + params.detailMeshes = polyMeshDetail.meshes; + params.detailVerts = polyMeshDetail.verts; + params.detailVertsCount = polyMeshDetail.nverts; + params.detailTris = polyMeshDetail.tris; + params.detailTriCount = polyMeshDetail.ntris; + params.offMeshConVerts = nullptr; + params.offMeshConRad = nullptr; + params.offMeshConDir = nullptr; + params.offMeshConAreas = nullptr; + params.offMeshConFlags = nullptr; + params.offMeshConUserID = nullptr; + params.offMeshConCount = 0; + params.walkableHeight = getHeight(settings, agentHalfExtents); + params.walkableRadius = getRadius(settings, agentHalfExtents); + params.walkableClimb = getMaxClimb(settings); + rcVcopy(params.bmin, polyMesh.bmin); + rcVcopy(params.bmax, polyMesh.bmax); + params.cs = config.cs; + params.ch = config.ch; + params.buildBvTree = true; + params.userId = 0; + params.tileX = tileX; + params.tileY = tileY; + params.tileLayer = 0; + + unsigned char* navMeshData; + int navMeshDataSize; + OPENMW_CHECK_DT_RESULT(dtCreateNavMeshData(¶ms, &navMeshData, &navMeshDataSize)); + + return NavMeshData(navMeshData, navMeshDataSize); + } + + int nextPow2(int v) + { + v--; + v |= v >> 1; + v |= v >> 2; + v |= v >> 4; + v |= v >> 8; + v |= v >> 16; + v++; + return v; + } + + NavMeshPtr makeNavMeshWithMultiTiles(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, + const Settings& settings) + { + osg::Vec3f boundsMin; + osg::Vec3f boundsMax; + rcCalcBounds(recastMesh.getVertices().data(), static_cast(recastMesh.getVerticesCount()), + boundsMin.ptr(), boundsMax.ptr()); + + const auto minTilePosition = getTilePosition(settings, boundsMin); + const auto maxTilePosition = getTilePosition(settings, boundsMax); + + const auto tileWidth = maxTilePosition.x() - minTilePosition.x() + 1; + const auto tileHeight = maxTilePosition.y() - minTilePosition.y() + 1; + + // Max tiles and max polys affect how the tile IDs are caculated. + // There are 22 bits available for identifying a tile and a polygon. + const auto tileBits = std::min(static_cast(std::log2(nextPow2(tileWidth * tileHeight))), 14); + const auto polyBits = 22 - tileBits; + const auto maxTiles = 1 << tileBits; + const auto maxPolysPerTile = 1 << polyBits; + + dtNavMeshParams params; + std::fill_n(params.orig, 3, 0.0f); + params.tileWidth = getTileSize(settings); + params.tileHeight = getTileSize(settings); + params.maxTiles = maxTiles; + params.maxPolys = maxPolysPerTile; + + NavMeshPtr navMesh(dtAllocNavMesh(), &dtFreeNavMesh); + OPENMW_CHECK_DT_STATUS(navMesh->init(¶ms)); + + for (int y = minTilePosition.y(); y <= maxTilePosition.y(); ++y) + { + for (int x = minTilePosition.x(); x <= maxTilePosition.x(); ++x) + { + const auto tileBounds = makeTileBounds(settings, TilePosition(x, y)); + const osg::Vec3f tileBorderMin(tileBounds.mMin.x(), boundsMin.y() - 1, tileBounds.mMin.y()); + const osg::Vec3f tileBorderMax(tileBounds.mMax.x(), boundsMax.y() + 1, tileBounds.mMax.y()); + + auto navMeshData = makeNavMeshTileData(agentHalfExtents, recastMesh, x, y, + tileBorderMin, tileBorderMax, settings); + + if (!navMeshData.mValue) + continue; + + OPENMW_CHECK_DT_STATUS(navMesh->addTile(navMeshData.mValue.get(), navMeshData.mSize, + DT_TILE_FREE_DATA, 0, 0)); + navMeshData.mValue.release(); + } + } + + return navMesh; + } +} + +namespace DetourNavigator +{ + NavMeshPtr makeNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, const Settings& settings) + { + return makeNavMeshWithMultiTiles(agentHalfExtents, recastMesh, settings); + } +} diff --git a/components/detournavigator/makenavmesh.hpp b/components/detournavigator/makenavmesh.hpp new file mode 100644 index 0000000000..a94e71c3f7 --- /dev/null +++ b/components/detournavigator/makenavmesh.hpp @@ -0,0 +1,20 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H + +#include + +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + class RecastMesh; + struct Settings; + + using NavMeshPtr = std::shared_ptr; + + NavMeshPtr makeNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, const Settings& settings); +} + +#endif diff --git a/components/detournavigator/navigator.cpp b/components/detournavigator/navigator.cpp new file mode 100644 index 0000000000..9f1d77a1cb --- /dev/null +++ b/components/detournavigator/navigator.cpp @@ -0,0 +1,42 @@ +#include "navigator.hpp" +#include "debug.hpp" +#include "settingsutils.hpp" + +namespace DetourNavigator +{ + Navigator::Navigator(const Settings& settings) + : mSettings(settings) + , mNavMeshManager(mSettings) + { + } + + void Navigator::addAgent(const osg::Vec3f& agentHalfExtents) + { + ++mAgents[agentHalfExtents]; + } + + void Navigator::removeAgent(const osg::Vec3f& agentHalfExtents) + { + const auto it = mAgents.find(agentHalfExtents); + if (it == mAgents.end() || --it->second) + return; + mAgents.erase(it); + mNavMeshManager.reset(agentHalfExtents); + } + + bool Navigator::removeObject(std::size_t id) + { + return mNavMeshManager.removeObject(id); + } + + void Navigator::update() + { + for (const auto& v : mAgents) + mNavMeshManager.update(v.first); + } + + void Navigator::wait() + { + mNavMeshManager.wait(); + } +} diff --git a/components/detournavigator/navigator.hpp b/components/detournavigator/navigator.hpp new file mode 100644 index 0000000000..779506a9a2 --- /dev/null +++ b/components/detournavigator/navigator.hpp @@ -0,0 +1,50 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H + +#include "findsmoothpath.hpp" +#include "navmeshmanager.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" + +namespace DetourNavigator +{ + class Navigator + { + public: + Navigator(const Settings& settings); + + void addAgent(const osg::Vec3f& agentHalfExtents); + + void removeAgent(const osg::Vec3f& agentHalfExtents); + + template + bool addObject(std::size_t id, const T& shape, const btTransform& transform) + { + return mNavMeshManager.addObject(id, shape, transform); + } + + bool removeObject(std::size_t id); + + void update(); + + void wait(); + + template + OutputIterator findPath(const osg::Vec3f& agentHalfExtents, const osg::Vec3f& start, + const osg::Vec3f& end, OutputIterator out) const + { + const auto navMesh = mNavMeshManager.getNavMesh(agentHalfExtents); + if (!navMesh) + return out; + return findSmoothPath(*navMesh, toNavMeshCoordinates(mSettings, agentHalfExtents), + toNavMeshCoordinates(mSettings, start), toNavMeshCoordinates(mSettings, end), mSettings, out); + } + + private: + Settings mSettings; + NavMeshManager mNavMeshManager; + std::map mAgents; + }; +} + +#endif diff --git a/components/detournavigator/navmeshmanager.cpp b/components/detournavigator/navmeshmanager.cpp new file mode 100644 index 0000000000..55e0236edb --- /dev/null +++ b/components/detournavigator/navmeshmanager.cpp @@ -0,0 +1,50 @@ +#include "navmeshmanager.hpp" +#include "debug.hpp" + +#include + +namespace DetourNavigator +{ + NavMeshManager::NavMeshManager(const Settings& settings) + : mRecastMeshManager(settings) + , mAsyncNavMeshUpdater(settings) + { + } + + bool NavMeshManager::removeObject(std::size_t id) + { + if (!mRecastMeshManager.removeObject(id)) + return false; + ++mRevision; + return true; + } + + void NavMeshManager::reset(const osg::Vec3f& agentHalfExtents) + { + mCache.erase(agentHalfExtents); + } + + void NavMeshManager::update(const osg::Vec3f& agentHalfExtents) + { + auto it = mCache.find(agentHalfExtents); + if (it == mCache.end()) + it = mCache.insert(std::make_pair(agentHalfExtents, std::make_shared(mRevision))).first; + else if (it->second->mRevision >= mRevision) + return; + it->second->mRevision = mRevision; + mAsyncNavMeshUpdater.post(agentHalfExtents, mRecastMeshManager.getMesh(), it->second); + } + + void NavMeshManager::wait() + { + mAsyncNavMeshUpdater.wait(); + } + + NavMeshConstPtr NavMeshManager::getNavMesh(const osg::Vec3f& agentHalfExtents) const + { + const auto it = mCache.find(agentHalfExtents); + if (it == mCache.end()) + return nullptr; + return it->second->mValue; + } +} diff --git a/components/detournavigator/navmeshmanager.hpp b/components/detournavigator/navmeshmanager.hpp new file mode 100644 index 0000000000..02309c6373 --- /dev/null +++ b/components/detournavigator/navmeshmanager.hpp @@ -0,0 +1,48 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHMANAGER_H + +#include "asyncnavmeshupdater.hpp" +#include "cachedrecastmeshmanager.hpp" + +#include + +#include +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + class NavMeshManager + { + public: + NavMeshManager(const Settings& settings); + + template + bool addObject(std::size_t id, const T& shape, const btTransform& transform) + { + if (!mRecastMeshManager.addObject(id, shape, transform)) + return false; + ++mRevision; + return true; + } + + bool removeObject(std::size_t id); + + void reset(const osg::Vec3f& agentHalfExtents); + + void update(const osg::Vec3f& agentHalfExtents); + + void wait(); + + NavMeshConstPtr getNavMesh(const osg::Vec3f& agentHalfExtents) const; + + private: + std::size_t mRevision = 0; + CachedRecastMeshManager mRecastMeshManager; + std::map> mCache; + AsyncNavMeshUpdater mAsyncNavMeshUpdater; + }; +} + +#endif diff --git a/components/detournavigator/recastmesh.cpp b/components/detournavigator/recastmesh.cpp new file mode 100644 index 0000000000..eaeea92f18 --- /dev/null +++ b/components/detournavigator/recastmesh.cpp @@ -0,0 +1,12 @@ +#include "recastmesh.hpp" +#include "settings.hpp" + +namespace DetourNavigator +{ + RecastMesh::RecastMesh(std::vector indices, std::vector vertices, const Settings& settings) + : mIndices(std::move(indices)) + , mVertices(std::move(vertices)) + , mChunkyTriMesh(mVertices, mIndices, settings.mTrianglesPerChunk) + { + } +} diff --git a/components/detournavigator/recastmesh.hpp b/components/detournavigator/recastmesh.hpp new file mode 100644 index 0000000000..35d5cb2472 --- /dev/null +++ b/components/detournavigator/recastmesh.hpp @@ -0,0 +1,50 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESH_H + +#include "chunkytrimesh.hpp" + +#include +#include + +namespace DetourNavigator +{ + struct Settings; + + class RecastMesh + { + public: + RecastMesh(std::vector indices, std::vector vertices, const Settings& settings); + + const std::vector& getIndices() const + { + return mIndices; + } + + const std::vector& getVertices() const + { + return mVertices; + } + + std::size_t getVerticesCount() const + { + return mVertices.size() / 3; + } + + std::size_t getTrianglesCount() const + { + return mIndices.size() / 3; + } + + const ChunkyTriMesh& getChunkyTriMesh() const + { + return mChunkyTriMesh; + } + + private: + std::vector mIndices; + std::vector mVertices; + ChunkyTriMesh mChunkyTriMesh; + }; +} + +#endif diff --git a/components/detournavigator/recastmeshbuilder.cpp b/components/detournavigator/recastmeshbuilder.cpp new file mode 100644 index 0000000000..83caa52878 --- /dev/null +++ b/components/detournavigator/recastmeshbuilder.cpp @@ -0,0 +1,72 @@ +#include "recastmeshbuilder.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" + +#include + +#include +#include + +namespace +{ + osg::Vec3f makeOsgVec3f(const btVector3& value) + { + return osg::Vec3f(value.x(), value.y(), value.z()); + } +} + +namespace DetourNavigator +{ + using BulletHelpers::makeProcessTriangleCallback; + + RecastMeshBuilder::RecastMeshBuilder(const Settings& settings) + : mSettings(settings) + { + } + + void RecastMeshBuilder::addObject(const btConcaveShape& shape, const btTransform& transform) + { + return addObject(shape, makeProcessTriangleCallback([&] (btVector3* triangle, int, int) + { + for (std::size_t i = 3; i > 0; --i) + addVertex(transform(triangle[i - 1])); + })); + } + + void RecastMeshBuilder::addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform) + { + return addObject(shape, makeProcessTriangleCallback([&] (btVector3* triangle, int, int) + { + for (std::size_t i = 0; i < 3; ++i) + addVertex(transform(triangle[i])); + })); + } + + std::shared_ptr RecastMeshBuilder::create() const + { + return std::make_shared(mIndices, mVertices, mSettings); + } + + void RecastMeshBuilder::reset() + { + mIndices.clear(); + mVertices.clear(); + } + + void RecastMeshBuilder::addObject(const btConcaveShape& shape, btTriangleCallback&& callback) + { + btVector3 aabbMin; + btVector3 aabbMax; + shape.getAabb(btTransform::getIdentity(), aabbMin, aabbMax); + shape.processAllTriangles(&callback, aabbMin, aabbMax); + } + + void RecastMeshBuilder::addVertex(const btVector3& worldPosition) + { + const auto navMeshPosition = toNavMeshCoordinates(mSettings, makeOsgVec3f(worldPosition)); + mIndices.push_back(static_cast(mIndices.size())); + mVertices.push_back(navMeshPosition.x()); + mVertices.push_back(navMeshPosition.y()); + mVertices.push_back(navMeshPosition.z()); + } +} diff --git a/components/detournavigator/recastmeshbuilder.hpp b/components/detournavigator/recastmeshbuilder.hpp new file mode 100644 index 0000000000..3236b63982 --- /dev/null +++ b/components/detournavigator/recastmeshbuilder.hpp @@ -0,0 +1,38 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHBUILDER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHBUILDER_H + +#include "recastmesh.hpp" + +class btConcaveShape; +class btHeightfieldTerrainShape; +class btTransform; +class btTriangleCallback; +class btVector3; + +namespace DetourNavigator +{ + class RecastMeshBuilder + { + public: + RecastMeshBuilder(const Settings& settings); + + void addObject(const btConcaveShape& shape, const btTransform& transform); + + void addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform); + + std::shared_ptr create() const; + + void reset(); + + private: + std::reference_wrapper mSettings; + std::vector mIndices; + std::vector mVertices; + + void addObject(const btConcaveShape& shape, btTriangleCallback&& callback); + + void addVertex(const btVector3& worldPosition); + }; +} + +#endif diff --git a/components/detournavigator/recastmeshmanager.cpp b/components/detournavigator/recastmeshmanager.cpp new file mode 100644 index 0000000000..25b3bda963 --- /dev/null +++ b/components/detournavigator/recastmeshmanager.cpp @@ -0,0 +1,57 @@ +#include "recastmeshmanager.hpp" + +#include + +namespace DetourNavigator +{ + RecastMeshManager::RecastMeshManager(const Settings& settings) + : mShouldRebuild(false) + , mMeshBuilder(settings) + { + } + + bool RecastMeshManager::addObject(std::size_t id, const btHeightfieldTerrainShape& shape, const btTransform& transform) + { + if (!mObjects.insert(std::make_pair(id, Object {&shape, transform})).second) + return false; + mShouldRebuild = true; + return true; + } + + bool RecastMeshManager::addObject(std::size_t id, const btConcaveShape& shape, const btTransform& transform) + { + if (!mObjects.insert(std::make_pair(id, Object {&shape, transform})).second) + return false; + mShouldRebuild = true; + return true; + } + + bool RecastMeshManager::removeObject(std::size_t id) + { + if (!mObjects.erase(id)) + return false; + mShouldRebuild = true; + return true; + } + + std::shared_ptr RecastMeshManager::getMesh() + { + rebuild(); + return mMeshBuilder.create(); + } + + void RecastMeshManager::rebuild() + { + if (!mShouldRebuild) + return; + mMeshBuilder.reset(); + for (const auto& v : mObjects) + { + if (v.second.mShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE) + mMeshBuilder.addObject(*static_cast(v.second.mShape), v.second.mTransform); + else if (v.second.mShape->isConcave()) + mMeshBuilder.addObject(*static_cast(v.second.mShape), v.second.mTransform); + } + mShouldRebuild = false; + } +} diff --git a/components/detournavigator/recastmeshmanager.hpp b/components/detournavigator/recastmeshmanager.hpp new file mode 100644 index 0000000000..12c7b391bc --- /dev/null +++ b/components/detournavigator/recastmeshmanager.hpp @@ -0,0 +1,42 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHMANAGER_H + +#include "recastmeshbuilder.hpp" + +#include + +#include + +class btCollisionShape; + +namespace DetourNavigator +{ + class RecastMeshManager + { + public: + RecastMeshManager(const Settings& settings); + + bool addObject(std::size_t id, const btHeightfieldTerrainShape& shape, const btTransform& transform); + + bool addObject(std::size_t id, const btConcaveShape& shape, const btTransform& transform); + + bool removeObject(std::size_t id); + + std::shared_ptr getMesh(); + + private: + struct Object + { + const btCollisionShape* mShape; + btTransform mTransform; + }; + + bool mShouldRebuild; + RecastMeshBuilder mMeshBuilder; + std::unordered_map mObjects; + + void rebuild(); + }; +} + +#endif diff --git a/components/detournavigator/settings.hpp b/components/detournavigator/settings.hpp new file mode 100644 index 0000000000..11e1f99904 --- /dev/null +++ b/components/detournavigator/settings.hpp @@ -0,0 +1,30 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGS_H + +#include + +namespace DetourNavigator +{ + struct Settings + { + float mCellHeight; + float mCellSize; + float mDetailSampleDist; + float mDetailSampleMaxError; + float mMaxClimb; + float mMaxSimplificationError; + float mMaxSlope; + float mRecastScaleFactor; + int mMaxEdgeLen; + int mMaxNavMeshQueryNodes; + int mMaxVertsPerPoly; + int mRegionMergeSize; + int mRegionMinSize; + int mTileSize; + std::size_t mMaxPolygonPathSize; + std::size_t mMaxSmoothPathSize; + std::size_t mTrianglesPerChunk; + }; +} + +#endif diff --git a/components/detournavigator/settingsutils.hpp b/components/detournavigator/settingsutils.hpp new file mode 100644 index 0000000000..d14863dca8 --- /dev/null +++ b/components/detournavigator/settingsutils.hpp @@ -0,0 +1,65 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGSUTILS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGSUTILS_H + +#include "settings.hpp" +#include "tileposition.hpp" +#include "tilebounds.hpp" + +#include + +#include + +namespace DetourNavigator +{ + inline float getHeight(const Settings& settings,const osg::Vec3f& agentHalfExtents) + { + return 2.0f * agentHalfExtents.z() * settings.mRecastScaleFactor; + } + + inline float getMaxClimb(const Settings& settings) + { + return settings.mMaxClimb * settings.mRecastScaleFactor; + } + + inline float getRadius(const Settings& settings, const osg::Vec3f& agentHalfExtents) + { + return agentHalfExtents.x() * settings.mRecastScaleFactor; + } + + inline osg::Vec3f toNavMeshCoordinates(const Settings& settings, osg::Vec3f position) + { + std::swap(position.y(), position.z()); + return position * settings.mRecastScaleFactor; + } + + inline osg::Vec3f fromNavMeshCoordinates(const Settings& settings, osg::Vec3f position) + { + const auto factor = 1.0f / settings.mRecastScaleFactor; + position *= factor; + std::swap(position.y(), position.z()); + return position; + } + + inline float getTileSize(const Settings& settings) + { + return settings.mTileSize * settings.mCellSize; + } + + inline TilePosition getTilePosition(const Settings& settings, const osg::Vec3f& position) + { + return TilePosition( + static_cast(std::floor(position.x() / getTileSize(settings))), + static_cast(std::floor(position.z() / getTileSize(settings))) + ); + } + + inline TileBounds makeTileBounds(const Settings& settings, const TilePosition& tilePosition) + { + return TileBounds { + osg::Vec2f(tilePosition.x(), tilePosition.y()) * getTileSize(settings), + osg::Vec2f(tilePosition.x() + 1, tilePosition.y() + 1) * getTileSize(settings), + }; + } +} + +#endif diff --git a/components/detournavigator/tilebounds.hpp b/components/detournavigator/tilebounds.hpp new file mode 100644 index 0000000000..83fe2b6296 --- /dev/null +++ b/components/detournavigator/tilebounds.hpp @@ -0,0 +1,15 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEBOUNDS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEBOUNDS_H + +#include + +namespace DetourNavigator +{ + struct TileBounds + { + osg::Vec2f mMin; + osg::Vec2f mMax; + }; +} + +#endif diff --git a/components/detournavigator/tileposition.hpp b/components/detournavigator/tileposition.hpp new file mode 100644 index 0000000000..ad7b226b61 --- /dev/null +++ b/components/detournavigator/tileposition.hpp @@ -0,0 +1,11 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEPOSITION_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEPOSITION_H + +#include + +namespace DetourNavigator +{ + using TilePosition = osg::Vec2i; +} + +#endif diff --git a/files/settings-default.cfg b/files/settings-default.cfg index ea6f6e7b66..008d34ee07 100644 --- a/files/settings-default.cfg +++ b/files/settings-default.cfg @@ -530,3 +530,63 @@ companion x = 0.6 companion y = 0.27 companion w = 0.38 companion h = 0.63 + +[Navigator] + +# Scale of NavMesh coordinates to world coordinates (value > 0.0). Recastnavigation builds voxels for world geometry. +# Basically voxel size is 1 / "cell size". To reduce amount of voxels we apply scale factor, to make voxel size +# "recast scale factor" / "cell size". Default value calculates by this equation: +# sStepSizeUp * "recast scale factor" / "cell size" = 3 (max climb height should be equal to 3 voxels) +recast scale factor = 0.017647058823529415 + +# The z-axis cell size to use for fields. (value > 0.0) +# Defines voxel/grid/cell size. So their values have significant +# side effects on all parameters defined in voxel units. +# The minimum value for this parameter depends on the platform's floating point +# accuracy, with the practical minimum usually around 0.05. +# Same default value is used in RecastDemo. +cell height = 0.2 + +# The xy-plane cell size to use for fields. (value > 0.0) +# Defines voxel/grid/cell size. So their values have significant +# side effects on all parameters defined in voxel units. +# The minimum value for this parameter depends on the platform's floating point +# accuracy, with the practical minimum usually around 0.05. +# Same default value is used in RecastDemo. +cell size = 0.2 + +# Sets the sampling distance to use when generating the detail mesh. (value = 0.0 or value >= 0.9) +detail sample dist = 6.0 + +# The maximum distance the detail mesh surface should deviate from heightfield data. (value >= 0.0) +detail sample max error = 1.0 + +# The maximum distance a simplfied contour's border edges should deviate the original raw contour. (value >= 0.0) +max simplification error = 1.3 + +# The width and height of each tile. (value > 0) +tile size = 64 + +# The maximum allowed length for contour edges along the border of the mesh. (value >= 0) +max edge len = 12 + +# Maximum number of search nodes. (0 < value <= 65535) +max nav mesh query nodes = 2048 + +# The maximum number of vertices allowed for polygons generated during the contour to polygon conversion process. (value >= 3) +max verts per poly = 6 + +# Any regions with a span count smaller than this value will, if possible, be merged with larger regions. (value >= 0) +region merge size = 20 + +# The minimum number of cells allowed to form isolated island areas. (value >= 0) +region min size = 8 + +# Maximum size of path over polygons (value > 0) +max polygon path size = 1024 + +# Maximum size of smoothed path (value > 0) +max smooth path size = 1024 + +# Maximum number of triangles in each node of mesh AABB tree (value > 0) +triangles per chunk = 256