diff --git a/CI/before_install.linux.sh b/CI/before_install.linux.sh index 25d05e619c..1ec88fb9e9 100755 --- a/CI/before_install.linux.sh +++ b/CI/before_install.linux.sh @@ -1,3 +1,3 @@ -#!/bin/sh +#!/bin/sh -e sudo ln -s /usr/bin/clang-3.6 /usr/local/bin/clang sudo ln -s /usr/bin/clang++-3.6 /usr/local/bin/clang++ diff --git a/CI/before_install.osx.sh b/CI/before_install.osx.sh index 51b4b90be9..da72563a27 100755 --- a/CI/before_install.osx.sh +++ b/CI/before_install.osx.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/sh -e brew update diff --git a/CI/before_script.linux.sh b/CI/before_script.linux.sh index dd879989a8..ff6f2218a7 100755 --- a/CI/before_script.linux.sh +++ b/CI/before_script.linux.sh @@ -5,6 +5,9 @@ free -m env GENERATOR='Unix Makefiles' CONFIGURATION=Release CI/build_googletest.sh GOOGLETEST_DIR="$(pwd)/googletest/build" +env GENERATOR='Unix Makefiles' CONFIGURATION=Release CI/build_recastnavigation.sh +RECASTNAVIGATION_DIR="$(pwd)/recastnavigation/build" + mkdir build cd build export CODE_COVERAGE=1 @@ -18,4 +21,5 @@ ${ANALYZE}cmake \ -DUSE_SYSTEM_TINYXML=TRUE \ -DGTEST_ROOT="${GOOGLETEST_DIR}" \ -DGMOCK_ROOT="${GOOGLETEST_DIR}" \ + -DRecastNavigation_ROOT="${RECASTNAVIGATION_DIR}" \ .. diff --git a/CI/before_script.msvc.sh b/CI/before_script.msvc.sh index 8ee4b7fd99..add6f0ace9 100644 --- a/CI/before_script.msvc.sh +++ b/CI/before_script.msvc.sh @@ -304,6 +304,10 @@ if ! [ -z $UNITY_BUILD ]; then add_cmake_opts "-DOPENMW_UNITY_BUILD=True" fi +if [ ${BITS} -eq 64 ]; then + GENERATOR="${GENERATOR} Win64" +fi + echo echo "===================================" echo "Starting prebuild on MSVC${MSVC_DISPLAY_YEAR} WIN${BITS}" @@ -449,7 +453,7 @@ fi else LIB_SUFFIX="0" fi - + add_cmake_opts -DBOOST_ROOT="$BOOST_SDK" \ -DBOOST_LIBRARYDIR="${BOOST_SDK}/lib${BITS}-msvc-${MSVC_VER}.${LIB_SUFFIX}" add_cmake_opts -DBoost_COMPILER="-${TOOLSET}" @@ -644,6 +648,13 @@ printf "SDL 2.0.7... " echo Done. } echo +# recastnavigation +printf 'recastnavigation...' +{ + env GENERATOR="${GENERATOR}" CONFIGURATION="${CONFIGURATION}" ${DEPS_INSTALL}/../../CI/build_recastnavigation.sh + add_cmake_opts -DRecastNavigation_ROOT="$(pwd)/recastnavigation/build" +} +echo cd $DEPS_INSTALL/.. echo echo "Setting up OpenMW build..." diff --git a/CI/before_script.osx.sh b/CI/before_script.osx.sh index 8ee01b652d..772a06ad49 100755 --- a/CI/before_script.osx.sh +++ b/CI/before_script.osx.sh @@ -1,8 +1,11 @@ -#!/bin/sh +#!/bin/sh -e export CXX=clang++ export CC=clang +env GENERATOR='Unix Makefiles' CONFIGURATION=Release CI/build_recastnavigation.sh +RECASTNAVIGATION_DIR="$(pwd)/recastnavigation/build" + DEPENDENCIES_ROOT="/private/tmp/openmw-deps/openmw-deps" QT_PATH=`brew --prefix qt` mkdir build @@ -17,5 +20,6 @@ cmake \ -D DESIRED_QT_VERSION=5 \ -D BUILD_ESMTOOL=FALSE \ -D BUILD_MYGUI_PLUGIN=FALSE \ +-D RecastNavigation_ROOT="${RECASTNAVIGATION_DIR}" \ -G"Unix Makefiles" \ .. diff --git a/CI/build_recastnavigation.sh b/CI/build_recastnavigation.sh new file mode 100755 index 0000000000..46f3c74808 --- /dev/null +++ b/CI/build_recastnavigation.sh @@ -0,0 +1,17 @@ +#!/bin/sh -e + +git clone https://github.com/recastnavigation/recastnavigation.git +cd recastnavigation +mkdir build +cd build +cmake \ + -DCMAKE_BUILD_TYPE="${CONFIGURATION}" \ + -DRECASTNAVIGATION_DEMO=OFF \ + -DRECASTNAVIGATION_TESTS=OFF \ + -DRECASTNAVIGATION_EXAMPLES=OFF \ + -DRECASTNAVIGATION_STATIC=ON \ + -DCMAKE_INSTALL_PREFIX=. \ + -G "${GENERATOR}" \ + .. +cmake --build . --config "${CONFIGURATION}" +cmake --build . --target install --config "${CONFIGURATION}" diff --git a/apps/openmw/CMakeLists.txt b/apps/openmw/CMakeLists.txt index 0092712dbc..ad8853753e 100644 --- a/apps/openmw/CMakeLists.txt +++ b/apps/openmw/CMakeLists.txt @@ -21,7 +21,7 @@ add_openmw_dir (mwrender actors objects renderingmanager animation rotatecontroller sky npcanimation vismask creatureanimation effectmanager util renderinginterface pathgrid rendermode weaponanimation bulletdebugdraw globalmap characterpreview camera localmap water terrainstorage ripplesimulation - renderbin actoranimation landmanager + renderbin actoranimation landmanager navmesh actorspaths ) add_openmw_dir (mwinput @@ -70,7 +70,7 @@ add_openmw_dir (mwworld ) add_openmw_dir (mwphysics - physicssystem trace collisiontype actor convert + physicssystem trace collisiontype actor convert object heightfield ) add_openmw_dir (mwclass @@ -117,6 +117,10 @@ include_directories( ${FFmpeg_INCLUDE_DIRS} ) +find_package(RecastNavigation COMPONENTS DebugUtils Detour Recast REQUIRED) + +include_directories(SYSTEM ${RecastNavigation_INCLUDE_DIRS}) + target_link_libraries(openmw ${OSG_LIBRARIES} ${OPENTHREADS_LIBRARIES} @@ -133,6 +137,7 @@ target_link_libraries(openmw ${FFmpeg_LIBRARIES} ${MyGUI_LIBRARIES} ${SDL2_LIBRARY} + ${RecastNavigation_LIBRARIES} "osg-ffmpeg-videoplayer" "oics" components diff --git a/apps/openmw/mwbase/world.hpp b/apps/openmw/mwbase/world.hpp index dedaad278a..0167cbfee7 100644 --- a/apps/openmw/mwbase/world.hpp +++ b/apps/openmw/mwbase/world.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -54,6 +55,11 @@ namespace MWMechanics struct Movement; } +namespace DetourNavigator +{ + class Navigator; +} + namespace MWWorld { class CellStore; @@ -595,6 +601,13 @@ namespace MWBase /// Preload VFX associated with this effect list virtual void preloadEffects(const ESM::EffectList* effectList) = 0; + + virtual DetourNavigator::Navigator* getNavigator() const = 0; + + virtual void updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const = 0; + + virtual void setNavMeshNumberToRender(const std::size_t value) = 0; }; } diff --git a/apps/openmw/mwmechanics/aiactivate.cpp b/apps/openmw/mwmechanics/aiactivate.cpp index d38a1299fd..f1d42ea1c4 100644 --- a/apps/openmw/mwmechanics/aiactivate.cpp +++ b/apps/openmw/mwmechanics/aiactivate.cpp @@ -36,7 +36,7 @@ namespace MWMechanics return true; //Target doesn't exist //Set the target destination for the actor - ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos; + const auto dest = target.getRefData().getPosition().asVec3(); if (pathTo(actor, dest, duration, MWBase::Environment::get().getWorld()->getMaxActivationDistance())) //Stop when you get in activation range { diff --git a/apps/openmw/mwmechanics/aicast.cpp b/apps/openmw/mwmechanics/aicast.cpp index 948ffb3aac..38dc2fa732 100644 --- a/apps/openmw/mwmechanics/aicast.cpp +++ b/apps/openmw/mwmechanics/aicast.cpp @@ -37,7 +37,7 @@ bool MWMechanics::AiCast::execute(const MWWorld::Ptr& actor, MWMechanics::Charac if (!target) return true; - if (!mManual && !pathTo(actor, target.getRefData().getPosition().pos, duration, mDistance)) + if (!mManual && !pathTo(actor, target.getRefData().getPosition().asVec3(), duration, mDistance)) { return false; } diff --git a/apps/openmw/mwmechanics/aicombat.cpp b/apps/openmw/mwmechanics/aicombat.cpp index a96832b698..ee7b4369e9 100644 --- a/apps/openmw/mwmechanics/aicombat.cpp +++ b/apps/openmw/mwmechanics/aicombat.cpp @@ -128,7 +128,7 @@ namespace MWMechanics float targetReachedTolerance = 0.0f; if (storage.mLOS) targetReachedTolerance = storage.mAttackRange; - bool is_target_reached = pathTo(actor, target.getRefData().getPosition().pos, duration, targetReachedTolerance); + const bool is_target_reached = pathTo(actor, target.getRefData().getPosition().asVec3(), duration, targetReachedTolerance); if (is_target_reached) storage.mReadyToAttack = true; } @@ -307,7 +307,7 @@ namespace MWMechanics osg::Vec3f localPos = actor.getRefData().getPosition().asVec3(); coords.toLocal(localPos); - int closestPointIndex = PathFinder::GetClosestPoint(pathgrid, localPos); + int closestPointIndex = PathFinder::getClosestPoint(pathgrid, localPos); for (int i = 0; i < static_cast(pathgrid->mPoints.size()); i++) { if (i != closestPointIndex && getPathGridGraph(storage.mCell).isPointConnected(closestPointIndex, i)) @@ -359,7 +359,7 @@ namespace MWMechanics float dist = (actor.getRefData().getPosition().asVec3() - target.getRefData().getPosition().asVec3()).length(); if ((dist > fFleeDistance && !storage.mLOS) - || pathTo(actor, storage.mFleeDest, duration)) + || pathTo(actor, PathFinder::makeOsgVec3(storage.mFleeDest), duration)) { state = AiCombatStorage::FleeState_Idle; } diff --git a/apps/openmw/mwmechanics/aiescort.cpp b/apps/openmw/mwmechanics/aiescort.cpp index a86d13d752..59c840a49a 100644 --- a/apps/openmw/mwmechanics/aiescort.cpp +++ b/apps/openmw/mwmechanics/aiescort.cpp @@ -100,7 +100,7 @@ namespace MWMechanics point.mAutogenerated = 0; point.mConnectionNum = 0; point.mUnknown = 0; - if (pathTo(actor,point,duration)) //Returns true on path complete + if (pathTo(actor, osg::Vec3f(mX, mY, mZ), duration)) //Returns true on path complete { mRemainingDuration = mDuration; return true; diff --git a/apps/openmw/mwmechanics/aifollow.cpp b/apps/openmw/mwmechanics/aifollow.cpp index a8f0713101..2bd6850354 100644 --- a/apps/openmw/mwmechanics/aifollow.cpp +++ b/apps/openmw/mwmechanics/aifollow.cpp @@ -163,7 +163,7 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte } //Set the target destination from the actor - ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos; + const auto dest = target.getRefData().getPosition().asVec3(); short baseFollowDistance = followDistance; short threshold = 30; // to avoid constant switching between moving/stopping @@ -196,7 +196,7 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte if (storage.mMoving) { //Check if you're far away - float dist = distance(dest, pos.pos[0], pos.pos[1], pos.pos[2]); + float dist = distance(dest, pos.asVec3()); if (dist > 450) actor.getClass().getCreatureStats(actor).setMovementFlag(MWMechanics::CreatureStats::Flag_Run, true); //Make NPC run diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp index 7045d28e54..a61b2fa37c 100644 --- a/apps/openmw/mwmechanics/aipackage.cpp +++ b/apps/openmw/mwmechanics/aipackage.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "../mwbase/world.hpp" #include "../mwbase/environment.hpp" @@ -25,7 +26,7 @@ MWMechanics::AiPackage::~AiPackage() {} -MWMechanics::AiPackage::AiPackage() : +MWMechanics::AiPackage::AiPackage() : mTimer(AI_REACTION_TIME + 1.0f), // to force initial pathbuild mTargetActorRefId(""), mTargetActorId(-1), @@ -90,70 +91,67 @@ void MWMechanics::AiPackage::reset() mTimer = AI_REACTION_TIME + 1.0f; mIsShortcutting = false; mShortcutProhibited = false; - mShortcutFailPos = ESM::Pathgrid::Point(); + mShortcutFailPos = osg::Vec3f(); mPathFinder.clearPath(); mObstacleCheck.clear(); } -bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest, float duration, float destTolerance) +bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance) { mTimer += duration; //Update timer - ESM::Position pos = actor.getRefData().getPosition(); //position of the actor + const auto position = actor.getRefData().getPosition().asVec3(); //position of the actor + const auto world = MWBase::Environment::get().getWorld(); + + { + const auto halfExtents = world->getHalfExtents(actor); + world->updateActorPath(actor, mPathFinder.getPath(), halfExtents, position, dest); + } /// Stops the actor when it gets too close to a unloaded cell //... At current time, this test is unnecessary. AI shuts down when actor is more than "actors processing range" setting value //... units from player, and exterior cells are 8192 units long and wide. //... But AI processing distance may increase in the future. - if (isNearInactiveCell(pos)) + if (isNearInactiveCell(position)) { actor.getClass().getMovementSettings(actor).mPosition[1] = 0; return false; } - // handle path building and shortcutting - ESM::Pathgrid::Point start = pos.pos; - - float distToTarget = distance(start, dest); - bool isDestReached = (distToTarget <= destTolerance); + const float distToTarget = distance(position, dest); + const bool isDestReached = (distToTarget <= destTolerance); if (!isDestReached && mTimer > AI_REACTION_TIME) { if (actor.getClass().isBipedal(actor)) openDoors(actor); - bool wasShortcutting = mIsShortcutting; + const bool wasShortcutting = mIsShortcutting; bool destInLOS = false; - - const MWWorld::Class& actorClass = actor.getClass(); - MWBase::World* world = MWBase::Environment::get().getWorld(); - - // check if actor can move along z-axis - bool actorCanMoveByZ = (actorClass.canSwim(actor) && MWBase::Environment::get().getWorld()->isSwimming(actor)) - || world->isFlying(actor); + const bool actorCanMoveByZ = canActorMoveByZAxis(actor); // Prohibit shortcuts for AiWander, if the actor can not move in 3 dimensions. - if (actorCanMoveByZ || getTypeId() != TypeIdWander) - mIsShortcutting = shortcutPath(start, dest, actor, &destInLOS, actorCanMoveByZ); // try to shortcut first + mIsShortcutting = actorCanMoveByZ + && shortcutPath(position, dest, actor, &destInLOS, actorCanMoveByZ); // try to shortcut first if (!mIsShortcutting) { if (wasShortcutting || doesPathNeedRecalc(dest, actor.getCell())) // if need to rebuild path { - mPathFinder.buildSyncedPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell())); + const auto playerHalfExtents = world->getHalfExtents(world->getPlayerPtr()); + mPathFinder.buildPath(actor, position, dest, actor.getCell(), getPathGridGraph(actor.getCell()), + playerHalfExtents, getNavigatorFlags(actor)); mRotateOnTheRunChecks = 3; // give priority to go directly on target if there is minimal opportunity if (destInLOS && mPathFinder.getPath().size() > 1) { // get point just before dest - std::list::const_iterator pPointBeforeDest = mPathFinder.getPath().end(); - --pPointBeforeDest; - --pPointBeforeDest; + auto pPointBeforeDest = mPathFinder.getPath().rbegin() + 1; // if start point is closer to the target then last point of path (excluding target itself) then go straight on the target - if (distance(start, dest) <= distance(dest, *pPointBeforeDest)) + if (distance(position, dest) <= distance(dest, *pPointBeforeDest)) { mPathFinder.clearPath(); mPathFinder.addPointToPath(dest); @@ -163,7 +161,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgr if (!mPathFinder.getPath().empty()) //Path has points in it { - ESM::Pathgrid::Point lastPos = mPathFinder.getPath().back(); //Get the end of the proposed path + const auto& lastPos = mPathFinder.getPath().back(); //Get the end of the proposed path if(distance(dest, lastPos) > 100) //End of the path is far from the destination mPathFinder.addPointToPath(dest); //Adds the final destination to the path, to try to get to where you want to go @@ -173,41 +171,41 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgr mTimer = 0; } - if (isDestReached || mPathFinder.checkPathCompleted(pos.pos[0], pos.pos[1])) // if path is finished + const auto pointTolerance = std::min(actor.getClass().getSpeed(actor), DEFAULT_TOLERANCE); + + mPathFinder.update(position, pointTolerance, destTolerance); + + if (isDestReached || mPathFinder.checkPathCompleted()) // if path is finished { // turn to destination point - zTurn(actor, getZAngleToPoint(start, dest)); - smoothTurn(actor, getXAngleToPoint(start, dest), 0); + zTurn(actor, getZAngleToPoint(position, dest)); + smoothTurn(actor, getXAngleToPoint(position, dest), 0); return true; } - else - { - if (mRotateOnTheRunChecks == 0 - || isReachableRotatingOnTheRun(actor, *mPathFinder.getPath().begin())) // to prevent circling around a path point - { - actor.getClass().getMovementSettings(actor).mPosition[1] = 1; // move to the target - if (mRotateOnTheRunChecks > 0) mRotateOnTheRunChecks--; - } - // handle obstacles on the way - evadeObstacles(actor, duration, pos); + if (mRotateOnTheRunChecks == 0 + || isReachableRotatingOnTheRun(actor, *mPathFinder.getPath().begin())) // to prevent circling around a path point + { + actor.getClass().getMovementSettings(actor).mPosition[1] = 1; // move to the target + if (mRotateOnTheRunChecks > 0) mRotateOnTheRunChecks--; } // turn to next path point by X,Z axes - zTurn(actor, mPathFinder.getZAngleToNext(pos.pos[0], pos.pos[1])); - smoothTurn(actor, mPathFinder.getXAngleToNext(pos.pos[0], pos.pos[1], pos.pos[2]), 0); + zTurn(actor, mPathFinder.getZAngleToNext(position.x(), position.y())); + smoothTurn(actor, mPathFinder.getXAngleToNext(position.x(), position.y(), position.z()), 0); + + mObstacleCheck.update(actor, duration); + + // handle obstacles on the way + evadeObstacles(actor); return false; } -void MWMechanics::AiPackage::evadeObstacles(const MWWorld::Ptr& actor, float duration, const ESM::Position& pos) +void MWMechanics::AiPackage::evadeObstacles(const MWWorld::Ptr& actor) { - zTurn(actor, mPathFinder.getZAngleToNext(pos.pos[0], pos.pos[1])); - - MWMechanics::Movement& movement = actor.getClass().getMovementSettings(actor); - // check if stuck due to obstacles - if (!mObstacleCheck.check(actor, duration)) return; + if (!mObstacleCheck.isEvading()) return; // first check if obstacle is a door static float distance = MWBase::Environment::get().getWorld()->getMaxActivationDistance(); @@ -219,13 +217,14 @@ void MWMechanics::AiPackage::evadeObstacles(const MWWorld::Ptr& actor, float dur } else { - mObstacleCheck.takeEvasiveAction(movement); + mObstacleCheck.takeEvasiveAction(actor.getClass().getMovementSettings(actor)); } } void MWMechanics::AiPackage::openDoors(const MWWorld::Ptr& actor) { - static float distance = MWBase::Environment::get().getWorld()->getMaxActivationDistance(); + const auto world = MWBase::Environment::get().getWorld(); + static float distance = world->getMaxActivationDistance(); const MWWorld::Ptr door = getNearbyDoor(actor, distance); if (door == MWWorld::Ptr()) @@ -236,7 +235,7 @@ void MWMechanics::AiPackage::openDoors(const MWWorld::Ptr& actor) { if ((door.getCellRef().getTrap().empty() && door.getCellRef().getLockLevel() <= 0 )) { - MWBase::Environment::get().getWorld()->activate(door, actor); + world->activate(door, actor); return; } @@ -248,7 +247,7 @@ void MWMechanics::AiPackage::openDoors(const MWWorld::Ptr& actor) MWWorld::Ptr keyPtr = invStore.search(keyId); if (!keyPtr.isEmpty()) - MWBase::Environment::get().getWorld()->activate(door, actor); + world->activate(door, actor); } } @@ -266,14 +265,15 @@ const MWMechanics::PathgridGraph& MWMechanics::AiPackage::getPathGridGraph(const return *cache[id].get(); } -bool MWMechanics::AiPackage::shortcutPath(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor, bool *destInLOS, bool isPathClear) +bool MWMechanics::AiPackage::shortcutPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::Ptr& actor, bool *destInLOS, bool isPathClear) { - if (!mShortcutProhibited || (PathFinder::MakeOsgVec3(mShortcutFailPos) - PathFinder::MakeOsgVec3(startPoint)).length() >= PATHFIND_SHORTCUT_RETRY_DIST) + if (!mShortcutProhibited || (mShortcutFailPos - startPoint).length() >= PATHFIND_SHORTCUT_RETRY_DIST) { // check if target is clearly visible isPathClear = !MWBase::Environment::get().getWorld()->castRay( - static_cast(startPoint.mX), static_cast(startPoint.mY), static_cast(startPoint.mZ), - static_cast(endPoint.mX), static_cast(endPoint.mY), static_cast(endPoint.mZ)); + startPoint.x(), startPoint.y(), startPoint.z(), + endPoint.x(), endPoint.y(), endPoint.z()); if (destInLOS != nullptr) *destInLOS = isPathClear; @@ -294,21 +294,22 @@ bool MWMechanics::AiPackage::shortcutPath(const ESM::Pathgrid::Point& startPoint return false; } -bool MWMechanics::AiPackage::checkWayIsClearForActor(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor) +bool MWMechanics::AiPackage::checkWayIsClearForActor(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::Ptr& actor) { - bool actorCanMoveByZ = (actor.getClass().canSwim(actor) && MWBase::Environment::get().getWorld()->isSwimming(actor)) - || MWBase::Environment::get().getWorld()->isFlying(actor); + const auto world = MWBase::Environment::get().getWorld(); + const bool actorCanMoveByZ = (actor.getClass().canSwim(actor) && world->isSwimming(actor)) + || world->isFlying(actor); if (actorCanMoveByZ) return true; - float actorSpeed = actor.getClass().getSpeed(actor); - float maxAvoidDist = AI_REACTION_TIME * actorSpeed + actorSpeed / MAX_VEL_ANGULAR_RADIANS * 2; // *2 - for reliability - osg::Vec3f::value_type distToTarget = osg::Vec3f(static_cast(endPoint.mX), static_cast(endPoint.mY), 0).length(); + const float actorSpeed = actor.getClass().getSpeed(actor); + const float maxAvoidDist = AI_REACTION_TIME * actorSpeed + actorSpeed / MAX_VEL_ANGULAR_RADIANS * 2; // *2 - for reliability + const float distToTarget = osg::Vec2f(endPoint.x(), endPoint.y()).length(); - float offsetXY = distToTarget > maxAvoidDist*1.5? maxAvoidDist : maxAvoidDist/2; + const float offsetXY = distToTarget > maxAvoidDist*1.5? maxAvoidDist : maxAvoidDist/2; - bool isClear = checkWayIsClear(PathFinder::MakeOsgVec3(startPoint), PathFinder::MakeOsgVec3(endPoint), offsetXY); + const bool isClear = checkWayIsClear(startPoint, endPoint, offsetXY); // update shortcut prohibit state if (isClear) @@ -316,12 +317,12 @@ bool MWMechanics::AiPackage::checkWayIsClearForActor(const ESM::Pathgrid::Point& if (mShortcutProhibited) { mShortcutProhibited = false; - mShortcutFailPos = ESM::Pathgrid::Point(); + mShortcutFailPos = osg::Vec3f(); } } if (!isClear) { - if (mShortcutFailPos.mX == 0 && mShortcutFailPos.mY == 0 && mShortcutFailPos.mZ == 0) + if (mShortcutFailPos == osg::Vec3f()) { mShortcutProhibited = true; mShortcutFailPos = startPoint; @@ -331,9 +332,11 @@ bool MWMechanics::AiPackage::checkWayIsClearForActor(const ESM::Pathgrid::Point& return isClear; } -bool MWMechanics::AiPackage::doesPathNeedRecalc(const ESM::Pathgrid::Point& newDest, const MWWorld::CellStore* currentCell) +bool MWMechanics::AiPackage::doesPathNeedRecalc(const osg::Vec3f& newDest, const MWWorld::CellStore* currentCell) { - return mPathFinder.getPath().empty() || (distance(mPathFinder.getPath().back(), newDest) > 10) || mPathFinder.getPathCell() != currentCell; + return mPathFinder.getPath().empty() + || (distance(mPathFinder.getPath().back(), newDest) > 10) + || mPathFinder.getPathCell() != currentCell; } bool MWMechanics::AiPackage::isTargetMagicallyHidden(const MWWorld::Ptr& target) @@ -343,14 +346,13 @@ bool MWMechanics::AiPackage::isTargetMagicallyHidden(const MWWorld::Ptr& target) || (magicEffects.get(ESM::MagicEffect::Chameleon).getMagnitude() > 75); } -bool MWMechanics::AiPackage::isNearInactiveCell(const ESM::Position& actorPos) +bool MWMechanics::AiPackage::isNearInactiveCell(osg::Vec3f position) { const ESM::Cell* playerCell(getPlayer().getCell()->getCell()); if (playerCell->isExterior()) { // get actor's distance from origin of center cell - osg::Vec3f actorOffset(actorPos.asVec3()); - CoordinateConverter(playerCell).toLocal(actorOffset); + CoordinateConverter(playerCell).toLocal(position); // currently assumes 3 x 3 grid for exterior cells, with player at center cell. // ToDo: (Maybe) use "exterior cell load distance" setting to get count of actual active cells @@ -358,8 +360,8 @@ bool MWMechanics::AiPackage::isNearInactiveCell(const ESM::Position& actorPos) const float distanceFromEdge = 200.0; float minThreshold = (-1.0f * ESM::Land::REAL_SIZE) + distanceFromEdge; float maxThreshold = (2.0f * ESM::Land::REAL_SIZE) - distanceFromEdge; - return (actorOffset[0] < minThreshold) || (maxThreshold < actorOffset[0]) - || (actorOffset[1] < minThreshold) || (maxThreshold < actorOffset[1]); + return (position.x() < minThreshold) || (maxThreshold < position.x()) + || (position.y() < minThreshold) || (maxThreshold < position.y()); } else { @@ -367,7 +369,7 @@ bool MWMechanics::AiPackage::isNearInactiveCell(const ESM::Position& actorPos) } } -bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest) +bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const osg::Vec3f& dest) { // get actor's shortest radius for moving in circle float speed = actor.getClass().getSpeed(actor); @@ -381,17 +383,40 @@ bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& act osg::Vec3f radiusDir = dir ^ osg::Z_AXIS; // radius is perpendicular to a tangent radiusDir.normalize(); radiusDir *= radius; - + // pick up the nearest center candidate - osg::Vec3f dest_ = PathFinder::MakeOsgVec3(dest); osg::Vec3f pos = actor.getRefData().getPosition().asVec3(); osg::Vec3f center1 = pos - radiusDir; osg::Vec3f center2 = pos + radiusDir; - osg::Vec3f center = (center1 - dest_).length2() < (center2 - dest_).length2() ? center1 : center2; + osg::Vec3f center = (center1 - dest).length2() < (center2 - dest).length2() ? center1 : center2; - float distToDest = (center - dest_).length(); + float distToDest = (center - dest).length(); // if pathpoint is reachable for the actor rotating on the run: // no points of actor's circle should be farther from the center than destination point return (radius <= distToDest); } + +DetourNavigator::Flags MWMechanics::AiPackage::getNavigatorFlags(const MWWorld::Ptr& actor) const +{ + const auto& actorClass = actor.getClass(); + DetourNavigator::Flags result = DetourNavigator::Flag_none; + + if (actorClass.isPureWaterCreature(actor) || (getTypeId() != TypeIdWander && actorClass.canSwim(actor))) + result |= DetourNavigator::Flag_swim; + + if (actorClass.canWalk(actor)) + result |= DetourNavigator::Flag_walk; + + if (actorClass.isBipedal(actor) && getTypeId() != TypeIdWander) + result |= DetourNavigator::Flag_openDoor; + + return result; +} + +bool MWMechanics::AiPackage::canActorMoveByZAxis(const MWWorld::Ptr& actor) const +{ + const auto world = MWBase::Environment::get().getWorld(); + const auto& actorClass = actor.getClass(); + return (actorClass.canSwim(actor) && world->isSwimming(actor)) || world->isFlying(actor); +} diff --git a/apps/openmw/mwmechanics/aipackage.hpp b/apps/openmw/mwmechanics/aipackage.hpp index 5a468ae30c..5be858b0bf 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; @@ -105,29 +107,35 @@ namespace MWMechanics bool isTargetMagicallyHidden(const MWWorld::Ptr& target); /// Return if actor's rotation speed is sufficient to rotate to the destination pathpoint on the run. Otherwise actor should rotate while standing. - static bool isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest); + static bool isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const osg::Vec3f& dest); protected: /// Handles path building and shortcutting with obstacles avoiding /** \return If the actor has arrived at his destination **/ - bool pathTo(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest, float duration, float destTolerance = 0.0f); + bool pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance = 0.0f); /// Check if there aren't any obstacles along the path to make shortcut possible /// If a shortcut is possible then path will be cleared and filled with the destination point. /// \param destInLOS If not nullptr function will return ray cast check result /// \return If can shortcut the path - bool shortcutPath(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor, bool *destInLOS, bool isPathClear); + bool shortcutPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::Ptr& actor, + bool *destInLOS, bool isPathClear); /// Check if the way to the destination is clear, taking into account actor speed - bool checkWayIsClearForActor(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor); + bool checkWayIsClearForActor(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::Ptr& actor); - virtual bool doesPathNeedRecalc(const ESM::Pathgrid::Point& newDest, const MWWorld::CellStore* currentCell); + bool doesPathNeedRecalc(const osg::Vec3f& newDest, const MWWorld::CellStore* currentCell); + + void evadeObstacles(const MWWorld::Ptr& actor); - void evadeObstacles(const MWWorld::Ptr& actor, float duration, const ESM::Position& pos); void openDoors(const MWWorld::Ptr& actor); const PathgridGraph& getPathGridGraph(const MWWorld::CellStore* cell); + DetourNavigator::Flags getNavigatorFlags(const MWWorld::Ptr& actor) const; + + bool canActorMoveByZAxis(const MWWorld::Ptr& actor) const; + // TODO: all this does not belong here, move into temporary storage PathFinder mPathFinder; ObstacleCheck mObstacleCheck; @@ -137,16 +145,14 @@ namespace MWMechanics std::string mTargetActorRefId; mutable int mTargetActorId; - osg::Vec3f mLastActorPos; - short mRotateOnTheRunChecks; // attempts to check rotation to the pathpoint on the run possibility bool mIsShortcutting; // if shortcutting at the moment bool mShortcutProhibited; // shortcutting may be prohibited after unsuccessful attempt - ESM::Pathgrid::Point mShortcutFailPos; // position of last shortcut fail + osg::Vec3f mShortcutFailPos; // position of last shortcut fail private: - bool isNearInactiveCell(const ESM::Position& actorPos); + bool isNearInactiveCell(osg::Vec3f position); }; } diff --git a/apps/openmw/mwmechanics/aipursue.cpp b/apps/openmw/mwmechanics/aipursue.cpp index f465946552..f746df3ffe 100644 --- a/apps/openmw/mwmechanics/aipursue.cpp +++ b/apps/openmw/mwmechanics/aipursue.cpp @@ -49,13 +49,13 @@ bool AiPursue::execute (const MWWorld::Ptr& actor, CharacterController& characte actor.getClass().getCreatureStats(actor).setDrawState(DrawState_Nothing); //Set the target desition from the actor - ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos; + const auto dest = target.getRefData().getPosition().asVec3(); ESM::Position aPos = actor.getRefData().getPosition(); float pathTolerance = 100.0; if (pathTo(actor, dest, duration, pathTolerance) && - std::abs(dest.mZ - aPos.pos[2]) < pathTolerance) // check the true distance in case the target is far away in Z-direction + std::abs(dest.z() - aPos.pos[2]) < pathTolerance) // check the true distance in case the target is far away in Z-direction { target.getClass().activate(target,actor).get()->execute(actor); //Arrest player when reached return true; diff --git a/apps/openmw/mwmechanics/aitravel.cpp b/apps/openmw/mwmechanics/aitravel.cpp index 90beb9ead5..cce9c0314c 100644 --- a/apps/openmw/mwmechanics/aitravel.cpp +++ b/apps/openmw/mwmechanics/aitravel.cpp @@ -58,8 +58,7 @@ namespace MWMechanics // Unfortunately, with vanilla assets destination is sometimes blocked by other actor. // If we got close to target, check for actors nearby. If they are, finish AI package. int destinationTolerance = 64; - ESM::Pathgrid::Point dest(static_cast(mX), static_cast(mY), static_cast(mZ)); - if (distance(pos.pos, dest) <= destinationTolerance) + if (distance(pos.asVec3(), osg::Vec3f(mX, mY, mZ)) <= destinationTolerance) { std::vector targetActors; std::pair result = MWBase::Environment::get().getWorld()->getHitContact(actor, destinationTolerance, targetActors); @@ -71,7 +70,7 @@ namespace MWMechanics } } - if (pathTo(actor, dest, duration)) + if (pathTo(actor, osg::Vec3f(mX, mY, mZ), duration)) { actor.getClass().getMovementSettings(actor).mPosition[1] = 0; return true; diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp index caacf8cb64..ad11f6d4e9 100644 --- a/apps/openmw/mwmechanics/aiwander.cpp +++ b/apps/openmw/mwmechanics/aiwander.cpp @@ -50,7 +50,8 @@ namespace MWMechanics AiWander::AiWander(int distance, int duration, int timeOfDay, const std::vector& idle, bool repeat): mDistance(distance), mDuration(duration), mRemainingDuration(duration), mTimeOfDay(timeOfDay), mIdle(idle), - mRepeat(repeat), mStoredInitialActorPosition(false), mInitialActorPosition(osg::Vec3f(0, 0, 0)), mHasDestination(false), mDestination(osg::Vec3f(0, 0, 0)) + mRepeat(repeat), mStoredInitialActorPosition(false), mInitialActorPosition(osg::Vec3f(0, 0, 0)), + mHasDestination(false), mDestination(osg::Vec3f(0, 0, 0)), mUsePathgrid(false) { mIdle.resize(8, 0); init(); @@ -151,16 +152,24 @@ namespace MWMechanics // rebuild a path to it if (!mPathFinder.isPathConstructed() && mHasDestination) { - ESM::Pathgrid::Point dest(PathFinder::MakePathgridPoint(mDestination)); - ESM::Pathgrid::Point start(PathFinder::MakePathgridPoint(pos)); - - mPathFinder.buildSyncedPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell())); + if (mUsePathgrid) + { + mPathFinder.buildPathByPathgrid(pos.asVec3(), mDestination, actor.getCell(), + getPathGridGraph(actor.getCell())); + } + else + { + const auto world = MWBase::Environment::get().getWorld(); + const auto playerHalfExtents = world->getHalfExtents(world->getPlayerPtr()); + mPathFinder.buildPath(actor, pos.asVec3(), mDestination, actor.getCell(), + getPathGridGraph(actor.getCell()), playerHalfExtents, getNavigatorFlags(actor)); + } if (mPathFinder.isPathConstructed()) storage.setState(AiWanderStorage::Wander_Walking); } - doPerFrameActionsForState(actor, duration, storage, pos); + doPerFrameActionsForState(actor, duration, storage); playIdleDialogueRandomly(actor); @@ -169,14 +178,14 @@ namespace MWMechanics if (AI_REACTION_TIME <= lastReaction) { lastReaction = 0; - return reactionTimeActions(actor, storage, currentCell, cellChange, pos, duration); + return reactionTimeActions(actor, storage, currentCell, cellChange, pos); } else return false; } bool AiWander::reactionTimeActions(const MWWorld::Ptr& actor, AiWanderStorage& storage, - const MWWorld::CellStore*& currentCell, bool cellChange, ESM::Position& pos, float duration) + const MWWorld::CellStore*& currentCell, bool cellChange, ESM::Position& pos) { if (mDistance <= 0) storage.mCanWanderAlongPathGrid = false; @@ -226,7 +235,7 @@ namespace MWMechanics } // If Wandering manually and hit an obstacle, stop - if (storage.mIsWanderingManually && mObstacleCheck.check(actor, duration, 2.0f)) { + if (storage.mIsWanderingManually && mObstacleCheck.isEvading()) { completeManualWalking(actor, storage); } @@ -251,7 +260,9 @@ namespace MWMechanics setPathToAnAllowedNode(actor, storage, pos); } } - } else if (storage.mIsWanderingManually && mPathFinder.checkPathCompleted(pos.pos[0], pos.pos[1], DESTINATION_TOLERANCE)) { + } + else if (storage.mIsWanderingManually && mPathFinder.checkPathCompleted()) + { completeManualWalking(actor, storage); } @@ -292,11 +303,9 @@ namespace MWMechanics * Commands actor to walk to a random location near original spawn location. */ void AiWander::wanderNearStart(const MWWorld::Ptr &actor, AiWanderStorage &storage, int wanderDistance) { - const ESM::Pathgrid::Point currentPosition = actor.getRefData().getPosition().pos; - const osg::Vec3f currentPositionVec3f = osg::Vec3f(currentPosition.mX, currentPosition.mY, currentPosition.mZ); + const auto currentPosition = actor.getRefData().getPosition().asVec3(); std::size_t attempts = 10; // If a unit can't wander out of water, don't want to hang here - ESM::Pathgrid::Point destinationPosition; bool isWaterCreature = actor.getClass().isPureWaterCreature(actor); do { // Determine a random location within radius of original position @@ -305,19 +314,24 @@ namespace MWMechanics const float destinationX = mInitialActorPosition.x() + wanderRadius * std::cos(randomDirection); const float destinationY = mInitialActorPosition.y() + wanderRadius * std::sin(randomDirection); const float destinationZ = mInitialActorPosition.z(); - destinationPosition = ESM::Pathgrid::Point(destinationX, destinationY, destinationZ); + const osg::Vec3f destinationPosition(destinationX, destinationY, destinationZ); mDestination = osg::Vec3f(destinationX, destinationY, destinationZ); // Check if land creature will walk onto water or if water creature will swim onto land if ((!isWaterCreature && !destinationIsAtWater(actor, mDestination)) || - (isWaterCreature && !destinationThroughGround(currentPositionVec3f, mDestination))) { - mPathFinder.buildSyncedPath(currentPosition, destinationPosition, actor.getCell(), getPathGridGraph(actor.getCell())); + (isWaterCreature && !destinationThroughGround(currentPosition, mDestination))) + { + const auto world = MWBase::Environment::get().getWorld();; + const auto playerHalfExtents = world->getHalfExtents(world->getPlayerPtr()); + mPathFinder.buildPath(actor, currentPosition, destinationPosition, actor.getCell(), + getPathGridGraph(actor.getCell()), playerHalfExtents, getNavigatorFlags(actor)); mPathFinder.addPointToPath(destinationPosition); if (mPathFinder.isPathConstructed()) { storage.setState(AiWanderStorage::Wander_Walking, true); mHasDestination = true; + mUsePathgrid = false; } return; } @@ -348,7 +362,7 @@ namespace MWMechanics storage.setState(AiWanderStorage::Wander_IdleNow); } - void AiWander::doPerFrameActionsForState(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage, ESM::Position& pos) + void AiWander::doPerFrameActionsForState(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage) { switch (storage.mState) { @@ -357,7 +371,7 @@ namespace MWMechanics break; case AiWanderStorage::Wander_Walking: - onWalkingStatePerFrameActions(actor, duration, storage, pos); + onWalkingStatePerFrameActions(actor, duration, storage); break; case AiWanderStorage::Wander_ChooseAction: @@ -413,11 +427,10 @@ namespace MWMechanics } } - void AiWander::onWalkingStatePerFrameActions(const MWWorld::Ptr& actor, - float duration, AiWanderStorage& storage, ESM::Position& pos) + void AiWander::onWalkingStatePerFrameActions(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage) { // Is there no destination or are we there yet? - if ((!mPathFinder.isPathConstructed()) || pathTo(actor, ESM::Pathgrid::Point(mPathFinder.getPath().back()), duration, DESTINATION_TOLERANCE)) + if ((!mPathFinder.isPathConstructed()) || pathTo(actor, osg::Vec3f(mPathFinder.getPath().back()), duration, DESTINATION_TOLERANCE)) { stopWalking(actor, storage); storage.setState(AiWanderStorage::Wander_ChooseAction); @@ -425,7 +438,7 @@ namespace MWMechanics else { // have not yet reached the destination - evadeObstacles(actor, storage, duration, pos); + evadeObstacles(actor, storage); } } @@ -456,7 +469,7 @@ namespace MWMechanics storage.setState(AiWanderStorage::Wander_IdleNow); } - void AiWander::evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage, float duration, ESM::Position& pos) + void AiWander::evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage) { if (mObstacleCheck.isEvading()) { @@ -592,15 +605,17 @@ namespace MWMechanics ToWorldCoordinates(dest, storage.mCell->getCell()); // actor position is already in world coordinates - ESM::Pathgrid::Point start(PathFinder::MakePathgridPoint(actorPos)); + const auto start = actorPos.asVec3(); // don't take shortcuts for wandering - mPathFinder.buildSyncedPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell())); + const auto destVec3f = PathFinder::makeOsgVec3(dest); + mPathFinder.buildPathByPathgrid(start, destVec3f, actor.getCell(), getPathGridGraph(actor.getCell())); if (mPathFinder.isPathConstructed()) { - mDestination = osg::Vec3f(dest.mX, dest.mY, dest.mZ); + mDestination = destVec3f; mHasDestination = true; + mUsePathgrid = true; // Remove this node as an option and add back the previously used node (stops NPC from picking the same node): ESM::Pathgrid::Point temp = storage.mAllowedNodes[randNode]; storage.mAllowedNodes.erase(storage.mAllowedNodes.begin() + randNode); @@ -631,15 +646,15 @@ namespace MWMechanics // Every now and then check whether one of the doors is opened. (maybe // at the end of playing idle?) If the door is opened then re-calculate // allowed nodes starting from the spawn point. - std::list paths = pathfinder.getPath(); + auto paths = pathfinder.getPath(); while(paths.size() >= 2) { - ESM::Pathgrid::Point pt = paths.back(); + const auto pt = paths.back(); for(unsigned int j = 0; j < nodes.size(); j++) { // FIXME: doesn't handle a door with the same X/Y // coordinates but with a different Z - if(nodes[j].mX == pt.mX && nodes[j].mY == pt.mY) + if (std::abs(nodes[j].mX - pt.x()) <= 0.5 && std::abs(nodes[j].mY - pt.y()) <= 0.5) { nodes.erase(nodes.begin() + j); break; @@ -731,7 +746,7 @@ namespace MWMechanics ESM::Pathgrid::Point worldDest = dest; ToWorldCoordinates(worldDest, actor.getCell()->getCell()); - bool isPathGridOccupied = MWBase::Environment::get().getMechanicsManager()->isAnyActorInRange(PathFinder::MakeOsgVec3(worldDest), 60); + bool isPathGridOccupied = MWBase::Environment::get().getMechanicsManager()->isAnyActorInRange(PathFinder::makeOsgVec3(worldDest), 60); // add offset only if the selected pathgrid is occupied by another actor if (isPathGridOccupied) @@ -752,18 +767,18 @@ namespace MWMechanics ESM::Pathgrid::Point connDest = points[randomIndex]; // add an offset towards random neighboring node - osg::Vec3f dir = PathFinder::MakeOsgVec3(connDest) - PathFinder::MakeOsgVec3(dest); + osg::Vec3f dir = PathFinder::makeOsgVec3(connDest) - PathFinder::makeOsgVec3(dest); float length = dir.length(); dir.normalize(); for (int j = 1; j <= 3; j++) { // move for 5-15% towards random neighboring node - dest = PathFinder::MakePathgridPoint(PathFinder::MakeOsgVec3(dest) + dir * (j * 5 * length / 100.f)); + dest = PathFinder::makePathgridPoint(PathFinder::makeOsgVec3(dest) + dir * (j * 5 * length / 100.f)); worldDest = dest; ToWorldCoordinates(worldDest, actor.getCell()->getCell()); - isOccupied = MWBase::Environment::get().getMechanicsManager()->isAnyActorInRange(PathFinder::MakeOsgVec3(worldDest), 60); + isOccupied = MWBase::Environment::get().getMechanicsManager()->isAnyActorInRange(PathFinder::makeOsgVec3(worldDest), 60); if (!isOccupied) break; @@ -799,7 +814,7 @@ namespace MWMechanics const ESM::Pathgrid *pathgrid = MWBase::Environment::get().getWorld()->getStore().get().search(*currentCell->getCell()); - int index = PathFinder::GetClosestPoint(pathgrid, PathFinder::MakeOsgVec3(dest)); + int index = PathFinder::getClosestPoint(pathgrid, PathFinder::makeOsgVec3(dest)); getPathGridGraph(currentCell).getNeighbouringPoints(index, points); } @@ -832,7 +847,7 @@ namespace MWMechanics CoordinateConverter(cell).toLocal(npcPos); // Find closest pathgrid point - int closestPointIndex = PathFinder::GetClosestPoint(pathgrid, npcPos); + int closestPointIndex = PathFinder::getClosestPoint(pathgrid, npcPos); // mAllowedNodes for this actor with pathgrid point indexes based on mDistance // and if the point is connected to the closest current point @@ -840,7 +855,7 @@ namespace MWMechanics int pointIndex = 0; for(unsigned int counter = 0; counter < pathgrid->mPoints.size(); counter++) { - osg::Vec3f nodePos(PathFinder::MakeOsgVec3(pathgrid->mPoints[counter])); + osg::Vec3f nodePos(PathFinder::makeOsgVec3(pathgrid->mPoints[counter])); if((npcPos - nodePos).length2() <= mDistance * mDistance && getPathGridGraph(cellStore).isPointConnected(closestPointIndex, counter)) { @@ -867,7 +882,7 @@ namespace MWMechanics // 2. Partway along the path between the point and its connected points. void AiWander::AddNonPathGridAllowedPoints(osg::Vec3f npcPos, const ESM::Pathgrid * pathGrid, int pointIndex, AiWanderStorage& storage) { - storage.mAllowedNodes.push_back(PathFinder::MakePathgridPoint(npcPos)); + storage.mAllowedNodes.push_back(PathFinder::makePathgridPoint(npcPos)); for (std::vector::const_iterator it = pathGrid->mEdges.begin(); it != pathGrid->mEdges.end(); ++it) { if (it->mV0 == pointIndex) @@ -879,8 +894,8 @@ namespace MWMechanics void AiWander::AddPointBetweenPathGridPoints(const ESM::Pathgrid::Point& start, const ESM::Pathgrid::Point& end, AiWanderStorage& storage) { - osg::Vec3f vectorStart = PathFinder::MakeOsgVec3(start); - osg::Vec3f delta = PathFinder::MakeOsgVec3(end) - vectorStart; + osg::Vec3f vectorStart = PathFinder::makeOsgVec3(start); + osg::Vec3f delta = PathFinder::makeOsgVec3(end) - vectorStart; float length = delta.length(); delta.normalize(); @@ -889,7 +904,7 @@ namespace MWMechanics // must not travel longer than distance between waypoints or NPC goes past waypoint distance = std::min(distance, static_cast(length)); delta *= distance; - storage.mAllowedNodes.push_back(PathFinder::MakePathgridPoint(vectorStart + delta)); + storage.mAllowedNodes.push_back(PathFinder::makePathgridPoint(vectorStart + delta)); } void AiWander::SetCurrentNodeToClosestAllowedNode(const osg::Vec3f& npcPos, AiWanderStorage& storage) @@ -898,7 +913,7 @@ namespace MWMechanics unsigned int index = 0; for (unsigned int counterThree = 0; counterThree < storage.mAllowedNodes.size(); counterThree++) { - osg::Vec3f nodePos(PathFinder::MakeOsgVec3(storage.mAllowedNodes[counterThree])); + osg::Vec3f nodePos(PathFinder::makeOsgVec3(storage.mAllowedNodes[counterThree])); float tempDist = (npcPos - nodePos).length2(); if (tempDist < distanceToClosestNode) { diff --git a/apps/openmw/mwmechanics/aiwander.hpp b/apps/openmw/mwmechanics/aiwander.hpp index 479ed4869c..bba6f71135 100644 --- a/apps/openmw/mwmechanics/aiwander.hpp +++ b/apps/openmw/mwmechanics/aiwander.hpp @@ -137,15 +137,15 @@ namespace MWMechanics short unsigned getRandomIdle(); void setPathToAnAllowedNode(const MWWorld::Ptr& actor, AiWanderStorage& storage, const ESM::Position& actorPos); void playGreetingIfPlayerGetsTooClose(const MWWorld::Ptr& actor, AiWanderStorage& storage); - void evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage, float duration, ESM::Position& pos); + void evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage); void playIdleDialogueRandomly(const MWWorld::Ptr& actor); void turnActorToFacePlayer(const osg::Vec3f& actorPosition, const osg::Vec3f& playerPosition, AiWanderStorage& storage); - void doPerFrameActionsForState(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage, ESM::Position& pos); + void doPerFrameActionsForState(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage); void onIdleStatePerFrameActions(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage); - void onWalkingStatePerFrameActions(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage, ESM::Position& pos); + void onWalkingStatePerFrameActions(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage); void onChooseActionStatePerFrameActions(const MWWorld::Ptr& actor, AiWanderStorage& storage); bool reactionTimeActions(const MWWorld::Ptr& actor, AiWanderStorage& storage, - const MWWorld::CellStore*& currentCell, bool cellChange, ESM::Position& pos, float duration); + const MWWorld::CellStore*& currentCell, bool cellChange, ESM::Position& pos); bool isPackageCompleted(const MWWorld::Ptr& actor, AiWanderStorage& storage); void wanderNearStart(const MWWorld::Ptr &actor, AiWanderStorage &storage, int wanderDistance); bool destinationIsAtWater(const MWWorld::Ptr &actor, const osg::Vec3f& destination); @@ -164,6 +164,7 @@ namespace MWMechanics bool mHasDestination; osg::Vec3f mDestination; + bool mUsePathgrid; void getNeighbouringNodes(ESM::Pathgrid::Point dest, const MWWorld::CellStore* currentCell, ESM::Pathgrid::PointList& points); diff --git a/apps/openmw/mwmechanics/coordinateconverter.cpp b/apps/openmw/mwmechanics/coordinateconverter.cpp index 0a2d99f924..04155ea493 100644 --- a/apps/openmw/mwmechanics/coordinateconverter.cpp +++ b/apps/openmw/mwmechanics/coordinateconverter.cpp @@ -33,11 +33,12 @@ namespace MWMechanics point.y() -= static_cast(mCellY); } - osg::Vec3f CoordinateConverter::toLocalVec3(const ESM::Pathgrid::Point& point) + osg::Vec3f CoordinateConverter::toLocalVec3(const osg::Vec3f& point) { return osg::Vec3f( - static_cast(point.mX - mCellX), - static_cast(point.mY - mCellY), - static_cast(point.mZ)); + point.x() - static_cast(mCellX), + point.y() - static_cast(mCellY), + point.z() + ); } } diff --git a/apps/openmw/mwmechanics/coordinateconverter.hpp b/apps/openmw/mwmechanics/coordinateconverter.hpp index cd855e84a2..f7dda33cb1 100644 --- a/apps/openmw/mwmechanics/coordinateconverter.hpp +++ b/apps/openmw/mwmechanics/coordinateconverter.hpp @@ -26,7 +26,7 @@ namespace MWMechanics /// in-place conversion from world to local void toLocal(osg::Vec3f& point); - osg::Vec3f toLocalVec3(const ESM::Pathgrid::Point& point); + osg::Vec3f toLocalVec3(const osg::Vec3f& point); private: int mCellX; diff --git a/apps/openmw/mwmechanics/obstacle.cpp b/apps/openmw/mwmechanics/obstacle.cpp index 0635a5520a..9562c74df0 100644 --- a/apps/openmw/mwmechanics/obstacle.cpp +++ b/apps/openmw/mwmechanics/obstacle.cpp @@ -96,11 +96,6 @@ namespace MWMechanics mEvadeDuration = 0; } - bool ObstacleCheck::isNormalState() const - { - return mWalkState == State_Norm; - } - bool ObstacleCheck::isEvading() const { return mWalkState == State_Evade; @@ -128,7 +123,7 @@ namespace MWMechanics * u = how long to move sideways * */ - bool ObstacleCheck::check(const MWWorld::Ptr& actor, float duration, float scaleMinimumDistance) + void ObstacleCheck::update(const MWWorld::Ptr& actor, float duration, float scaleMinimumDistance) { const MWWorld::Class& cls = actor.getClass(); ESM::Position pos = actor.getRefData().getPosition(); @@ -180,9 +175,7 @@ namespace MWMechanics case State_Evade: { mEvadeDuration += duration; - if(mEvadeDuration < DURATION_TO_EVADE) - return true; - else + if(mEvadeDuration >= DURATION_TO_EVADE) { // tried to evade, assume all is ok and start again mWalkState = State_Norm; @@ -191,10 +184,9 @@ namespace MWMechanics } /* NO DEFAULT CASE */ } - return false; // no obstacles to evade (yet) } - void ObstacleCheck::takeEvasiveAction(MWMechanics::Movement& actorMovement) + void ObstacleCheck::takeEvasiveAction(MWMechanics::Movement& actorMovement) const { actorMovement.mPosition[0] = evadeDirections[mEvadeDirectionIndex][0]; actorMovement.mPosition[1] = evadeDirections[mEvadeDirectionIndex][1]; diff --git a/apps/openmw/mwmechanics/obstacle.hpp b/apps/openmw/mwmechanics/obstacle.hpp index c553745014..d7e582f8c5 100644 --- a/apps/openmw/mwmechanics/obstacle.hpp +++ b/apps/openmw/mwmechanics/obstacle.hpp @@ -27,15 +27,13 @@ namespace MWMechanics // Clear the timers and set the state machine to default void clear(); - bool isNormalState() const; bool isEvading() const; - // Returns true if there is an obstacle and an evasive action - // should be taken - bool check(const MWWorld::Ptr& actor, float duration, float scaleMinimumDistance = 1.0f); + // Updates internal state, call each frame for moving actor + void update(const MWWorld::Ptr& actor, float duration, float scaleMinimumDistance = 1.0f); // change direction to try to fix "stuck" actor - void takeEvasiveAction(MWMechanics::Movement& actorMovement); + void takeEvasiveAction(MWMechanics::Movement& actorMovement) const; private: diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index c16cff9e10..8c7d6fce93 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -1,13 +1,20 @@ #include "pathfinding.hpp" +#include #include +#include +#include +#include +#include + #include "../mwbase/world.hpp" #include "../mwbase/environment.hpp" #include "../mwphysics/collisiontype.hpp" #include "../mwworld/cellstore.hpp" +#include "../mwworld/class.hpp" #include "pathgrid.hpp" #include "coordinateconverter.hpp" @@ -29,7 +36,7 @@ namespace // points to a quadtree may help for(unsigned int counter = 0; counter < grid->mPoints.size(); counter++) { - float potentialDistBetween = MWMechanics::PathFinder::DistanceSquared(grid->mPoints[counter], pos); + float potentialDistBetween = MWMechanics::PathFinder::distanceSquared(grid->mPoints[counter], pos); if (potentialDistBetween < closestDistanceReachable) { // found a closer one @@ -57,56 +64,19 @@ namespace (closestReachableIndex, closestReachableIndex == closestIndex); } + float sqrDistance(const osg::Vec2f& lhs, const osg::Vec2f& rhs) + { + return (lhs - rhs).length2(); + } + + float sqrDistanceIgnoreZ(const osg::Vec3f& lhs, const osg::Vec3f& rhs) + { + return sqrDistance(osg::Vec2f(lhs.x(), lhs.y()), osg::Vec2f(rhs.x(), rhs.y())); + } } namespace MWMechanics { - float sqrDistanceIgnoreZ(const ESM::Pathgrid::Point& point, float x, float y) - { - x -= point.mX; - y -= point.mY; - return (x * x + y * y); - } - - float distance(const ESM::Pathgrid::Point& point, float x, float y, float z) - { - x -= point.mX; - y -= point.mY; - z -= point.mZ; - return sqrt(x * x + y * y + z * z); - } - - float distance(const ESM::Pathgrid::Point& a, const ESM::Pathgrid::Point& b) - { - float x = static_cast(a.mX - b.mX); - float y = static_cast(a.mY - b.mY); - float z = static_cast(a.mZ - b.mZ); - return sqrt(x * x + y * y + z * z); - } - - float getZAngleToDir(const osg::Vec3f& dir) - { - return std::atan2(dir.x(), dir.y()); - } - - float getXAngleToDir(const osg::Vec3f& dir) - { - float dirLen = dir.length(); - return (dirLen != 0) ? -std::asin(dir.z() / dirLen) : 0; - } - - float getZAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest) - { - osg::Vec3f dir = PathFinder::MakeOsgVec3(dest) - PathFinder::MakeOsgVec3(origin); - return getZAngleToDir(dir); - } - - float getXAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest) - { - osg::Vec3f dir = PathFinder::MakeOsgVec3(dest) - PathFinder::MakeOsgVec3(origin); - return getXAngleToDir(dir); - } - bool checkWayIsClear(const osg::Vec3f& from, const osg::Vec3f& to, float offsetXY) { osg::Vec3f dir = to - from; @@ -121,18 +91,6 @@ namespace MWMechanics return (std::abs(from.z() - h) <= PATHFIND_Z_REACH); } - PathFinder::PathFinder() - : mPathgrid(nullptr) - , mCell(nullptr) - { - } - - void PathFinder::clearPath() - { - if(!mPath.empty()) - mPath.clear(); - } - /* * NOTE: This method may fail to find a path. The caller must check the * result before using it. If there is no path the AI routies need to @@ -150,7 +108,7 @@ namespace MWMechanics * pathgrid point (e.g. wander) then it may be worth while to call * pop_back() to remove the redundant entry. * - * NOTE: coordinates must be converted prior to calling GetClosestPoint() + * NOTE: coordinates must be converted prior to calling getClosestPoint() * * | * | cell @@ -169,52 +127,44 @@ namespace MWMechanics * j = @.x in local coordinates (i.e. within the cell) * k = @.x in world coordinates */ - void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint, - const ESM::Pathgrid::Point &endPoint, - const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph) + void PathFinder::buildPathByPathgridImpl(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const PathgridGraph& pathgridGraph, std::back_insert_iterator> out) { - mPath.clear(); - - // TODO: consider removing mCell / mPathgrid in favor of mPathgridGraph - if(mCell != cell || !mPathgrid) - { - mCell = cell; - mPathgrid = pathgridGraph.getPathgrid(); - } + const auto pathgrid = pathgridGraph.getPathgrid(); // Refer to AiWander reseach topic on openmw forums for some background. // Maybe there is no pathgrid for this cell. Just go to destination and let // physics take care of any blockages. - if(!mPathgrid || mPathgrid->mPoints.empty()) + if(!pathgrid || pathgrid->mPoints.empty()) { - mPath.push_back(endPoint); + *out++ = endPoint; return; } - // NOTE: GetClosestPoint expects local coordinates + // NOTE: getClosestPoint expects local coordinates CoordinateConverter converter(mCell->getCell()); - // NOTE: It is possible that GetClosestPoint returns a pathgrind point index + // NOTE: It is possible that getClosestPoint returns a pathgrind point index // that is unreachable in some situations. e.g. actor is standing // outside an area enclosed by walls, but there is a pathgrid // point right behind the wall that is closer than any pathgrid // point outside the wall osg::Vec3f startPointInLocalCoords(converter.toLocalVec3(startPoint)); - int startNode = GetClosestPoint(mPathgrid, startPointInLocalCoords); + int startNode = getClosestPoint(pathgrid, startPointInLocalCoords); osg::Vec3f endPointInLocalCoords(converter.toLocalVec3(endPoint)); - std::pair endNode = getClosestReachablePoint(mPathgrid, &pathgridGraph, + std::pair endNode = getClosestReachablePoint(pathgrid, &pathgridGraph, endPointInLocalCoords, startNode); // if it's shorter for actor to travel from start to end, than to travel from either // start or end to nearest pathgrid point, just travel from start to end. float startToEndLength2 = (endPointInLocalCoords - startPointInLocalCoords).length2(); - float endTolastNodeLength2 = DistanceSquared(mPathgrid->mPoints[endNode.first], endPointInLocalCoords); - float startTo1stNodeLength2 = DistanceSquared(mPathgrid->mPoints[startNode], startPointInLocalCoords); + float endTolastNodeLength2 = distanceSquared(pathgrid->mPoints[endNode.first], endPointInLocalCoords); + float startTo1stNodeLength2 = distanceSquared(pathgrid->mPoints[startNode], startPointInLocalCoords); if ((startToEndLength2 < startTo1stNodeLength2) || (startToEndLength2 < endTolastNodeLength2)) { - mPath.push_back(endPoint); + *out++ = endPoint; return; } @@ -225,21 +175,21 @@ namespace MWMechanics // nodes are the same if(startNode == endNode.first) { - ESM::Pathgrid::Point temp(mPathgrid->mPoints[startNode]); + ESM::Pathgrid::Point temp(pathgrid->mPoints[startNode]); converter.toWorld(temp); - mPath.push_back(temp); + *out++ = makeOsgVec3(temp); } else { - mPath = pathgridGraph.aStarSearch(startNode, endNode.first); + auto path = pathgridGraph.aStarSearch(startNode, endNode.first); // If nearest path node is in opposite direction from second, remove it from path. // Especially useful for wandering actors, if the nearest node is blocked for some reason. - if (mPath.size() > 1) + if (path.size() > 1) { - ESM::Pathgrid::Point secondNode = *(++mPath.begin()); - osg::Vec3f firstNodeVec3f = MakeOsgVec3(mPathgrid->mPoints[startNode]); - osg::Vec3f secondNodeVec3f = MakeOsgVec3(secondNode); + ESM::Pathgrid::Point secondNode = *(++path.begin()); + osg::Vec3f firstNodeVec3f = makeOsgVec3(pathgrid->mPoints[startNode]); + osg::Vec3f secondNodeVec3f = makeOsgVec3(secondNode); osg::Vec3f toSecondNodeVec3f = secondNodeVec3f - firstNodeVec3f; osg::Vec3f toStartPointVec3f = startPointInLocalCoords - firstNodeVec3f; if (toSecondNodeVec3f * toStartPointVec3f > 0) @@ -248,23 +198,25 @@ namespace MWMechanics converter.toWorld(temp); // Add Z offset since path node can overlap with other objects. // Also ignore doors in raytesting. - int mask = MWPhysics::CollisionType_World; + const int mask = MWPhysics::CollisionType_World; bool isPathClear = !MWBase::Environment::get().getWorld()->castRay( - startPoint.mX, startPoint.mY, startPoint.mZ+16, temp.mX, temp.mY, temp.mZ+16, mask); + startPoint.x(), startPoint.y(), startPoint.z() + 16, temp.mX, temp.mY, temp.mZ + 16, mask); if (isPathClear) - mPath.pop_front(); + path.pop_front(); } } // convert supplied path to world coordinates - for (std::list::iterator iter(mPath.begin()); iter != mPath.end(); ++iter) - { - converter.toWorld(*iter); - } + std::transform(path.begin(), path.end(), out, + [&] (ESM::Pathgrid::Point& point) + { + converter.toWorld(point); + return makeOsgVec3(point); + }); } // If endNode found is NOT the closest PathGrid point to the endPoint, - // assume endPoint is not reachable from endNode. In which case, + // assume endPoint is not reachable from endNode. In which case, // path ends at endNode. // // So only add the destination (which may be different to the closest @@ -276,7 +228,7 @@ namespace MWMechanics // // The AI routines will have to deal with such situations. if(endNode.second) - mPath.push_back(endPoint); + *out++ = endPoint; } float PathFinder::getZAngleToNext(float x, float y) const @@ -286,9 +238,9 @@ namespace MWMechanics if(mPath.empty()) return 0.; - const ESM::Pathgrid::Point &nextPoint = *mPath.begin(); - float directionX = nextPoint.mX - x; - float directionY = nextPoint.mY - y; + const auto& nextPoint = mPath.front(); + const float directionX = nextPoint.x() - x; + const float directionY = nextPoint.y() - y; return std::atan2(directionX, directionY); } @@ -300,62 +252,64 @@ namespace MWMechanics if(mPath.empty()) return 0.; - const ESM::Pathgrid::Point &nextPoint = *mPath.begin(); - osg::Vec3f dir = MakeOsgVec3(nextPoint) - osg::Vec3f(x,y,z); + const osg::Vec3f dir = mPath.front() - osg::Vec3f(x, y, z); return getXAngleToDir(dir); } - bool PathFinder::checkPathCompleted(float x, float y, float tolerance) + void PathFinder::update(const osg::Vec3f& position, const float pointTolerance, const float destinationTolerance) { - if(mPath.empty()) - return true; + if (mPath.empty()) + return; - const ESM::Pathgrid::Point& nextPoint = *mPath.begin(); - if (sqrDistanceIgnoreZ(nextPoint, x, y) < tolerance*tolerance) - { + const auto tolerance = mPath.size() > 1 ? pointTolerance : destinationTolerance; + + if (sqrDistanceIgnoreZ(mPath.front(), position) < tolerance * tolerance) mPath.pop_front(); - if(mPath.empty()) - { - return true; - } - } - - return false; } - // see header for the rationale - void PathFinder::buildSyncedPath(const ESM::Pathgrid::Point &startPoint, - const ESM::Pathgrid::Point &endPoint, - const MWWorld::CellStore* cell, const MWMechanics::PathgridGraph& pathgridGraph) + void PathFinder::buildPathByPathgrid(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph) { - if (mPath.size() < 2) - { - // if path has one point, then it's the destination. - // don't need to worry about bad path for this case - buildPath(startPoint, endPoint, cell, pathgridGraph); - } - else - { - const ESM::Pathgrid::Point oldStart(*getPath().begin()); - buildPath(startPoint, endPoint, cell, pathgridGraph); - if (mPath.size() >= 2) - { - // if 2nd waypoint of new path == 1st waypoint of old, - // delete 1st waypoint of new path. - std::list::iterator iter = ++mPath.begin(); - if (iter->mX == oldStart.mX - && iter->mY == oldStart.mY - && iter->mZ == oldStart.mZ) - { - mPath.pop_front(); - } - } - } + mPath.clear(); + mCell = cell; + + buildPathByPathgridImpl(startPoint, endPoint, pathgridGraph, std::back_inserter(mPath)); + + mConstructed = true; } - const MWWorld::CellStore* PathFinder::getPathCell() const + void PathFinder::buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, + const DetourNavigator::Flags flags) { - return mCell; + mPath.clear(); + mCell = cell; + + buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, std::back_inserter(mPath)); + + if (mPath.empty()) + buildPathByPathgridImpl(startPoint, endPoint, pathgridGraph, std::back_inserter(mPath)); + + mConstructed = true; + } + + void PathFinder::buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, + const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, + std::back_insert_iterator> out) + { + try + { + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + navigator->findPath(halfExtents, startPoint, endPoint, flags, out); + } + catch (const DetourNavigator::NavigatorException& exception) + { + DetourNavigator::log("PathFinder::buildPathByNavigator navigator exception: ", exception.what()); + Log(Debug::Verbose) << "Build path by navigator exception: \"" << exception.what() + << "\" for \"" << actor.getClass().getName(actor) << "\" (" << actor.getBase() + << ") from " << startPoint << " to " << endPoint << " with flags (" + << DetourNavigator::WriteFlags {flags} << ")"; + } } } diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index ebc22f10d4..567056fa5d 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -1,33 +1,57 @@ #ifndef GAME_MWMECHANICS_PATHFINDING_H #define GAME_MWMECHANICS_PATHFINDING_H -#include +#include #include +#include +#include #include #include namespace MWWorld { class CellStore; + class ConstPtr; } namespace MWMechanics { class PathgridGraph; - float distance(const ESM::Pathgrid::Point& point, float x, float y, float); - float distance(const ESM::Pathgrid::Point& a, const ESM::Pathgrid::Point& b); - float getZAngleToDir(const osg::Vec3f& dir); - float getXAngleToDir(const osg::Vec3f& dir); - float getZAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest); - float getXAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest); + inline float distance(const osg::Vec3f& lhs, const osg::Vec3f& rhs) + { + return (lhs - rhs).length(); + } - const float PATHFIND_Z_REACH = 50.0f; + inline float getZAngleToDir(const osg::Vec3f& dir) + { + return std::atan2(dir.x(), dir.y()); + } + + inline float getXAngleToDir(const osg::Vec3f& dir) + { + float dirLen = dir.length(); + return (dirLen != 0) ? -std::asin(dir.z() / dirLen) : 0; + } + + inline float getZAngleToPoint(const osg::Vec3f& origin, const osg::Vec3f& dest) + { + return getZAngleToDir(dest - origin); + } + + inline float getXAngleToPoint(const osg::Vec3f& origin, const osg::Vec3f& dest) + { + return getXAngleToDir(dest - origin); + } + + const float PATHFIND_Z_REACH = 50.0f; //static const float sMaxSlope = 49.0f; // duplicate as in physicssystem // distance after which actor (failed previously to shortcut) will try again const float PATHFIND_SHORTCUT_RETRY_DIST = 300.0f; + const float DEFAULT_TOLERANCE = 32.0f; + // cast up-down ray with some offset from actor position to check for pits/obstacles on the way to target; // magnitude of pits/obstacles is defined by PATHFIND_Z_REACH bool checkWayIsClear(const osg::Vec3f& from, const osg::Vec3f& to, float offsetXY); @@ -35,31 +59,33 @@ namespace MWMechanics class PathFinder { public: - PathFinder(); - - static const int PathTolerance = 32; - - static float sgn(float val) + PathFinder() + : mConstructed(false) + , mCell(nullptr) { - if(val > 0) - return 1.0; - return -1.0; } - static int sgn(int a) + void clearPath() { - if(a > 0) - return 1; - return -1; + mConstructed = false; + mPath.clear(); + mCell = nullptr; } - void clearPath(); + void buildPathByPathgrid(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph); - void buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint, - const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph); + void buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, + const DetourNavigator::Flags flags); - bool checkPathCompleted(float x, float y, float tolerance = PathTolerance); - ///< \Returns true if we are within \a tolerance units of the last path point. + /// Remove front point if exist and within tolerance + void update(const osg::Vec3f& position, const float pointTolerance, const float destinationTolerance); + + bool checkPathCompleted() const + { + return mConstructed && mPath.empty(); + } /// In radians float getZAngleToNext(float x, float y) const; @@ -68,60 +94,54 @@ namespace MWMechanics bool isPathConstructed() const { - return !mPath.empty(); + return mConstructed && !mPath.empty(); } - int getPathSize() const + std::size_t getPathSize() const { return mPath.size(); } - const std::list& getPath() const + const std::deque& getPath() const { return mPath; } - const MWWorld::CellStore* getPathCell() const; - - /** Synchronize new path with old one to avoid visiting 1 waypoint 2 times - @note - BuildPath() takes closest PathGrid point to NPC as first point of path. - This is undesirable if NPC has just passed a Pathgrid point, as this - makes the 2nd point of the new path == the 1st point of old path. - Which results in NPC "running in a circle" back to the just passed waypoint. - */ - void buildSyncedPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint, - const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph); - - void addPointToPath(const ESM::Pathgrid::Point &point) + const MWWorld::CellStore* getPathCell() const { + return mCell; + } + + void addPointToPath(const osg::Vec3f& point) + { + mConstructed = true; mPath.push_back(point); } /// utility function to convert a osg::Vec3f to a Pathgrid::Point - static ESM::Pathgrid::Point MakePathgridPoint(const osg::Vec3f& v) + static ESM::Pathgrid::Point makePathgridPoint(const osg::Vec3f& v) { return ESM::Pathgrid::Point(static_cast(v[0]), static_cast(v[1]), static_cast(v[2])); } /// utility function to convert an ESM::Position to a Pathgrid::Point - static ESM::Pathgrid::Point MakePathgridPoint(const ESM::Position& p) + static ESM::Pathgrid::Point makePathgridPoint(const ESM::Position& p) { return ESM::Pathgrid::Point(static_cast(p.pos[0]), static_cast(p.pos[1]), static_cast(p.pos[2])); } - static osg::Vec3f MakeOsgVec3(const ESM::Pathgrid::Point& p) + static osg::Vec3f makeOsgVec3(const ESM::Pathgrid::Point& p) { return osg::Vec3f(static_cast(p.mX), static_cast(p.mY), static_cast(p.mZ)); } - + // Slightly cheaper version for comparisons. // Caller needs to be careful for very short distances (i.e. less than 1) // or when accumuating the results i.e. (a + b)^2 != a^2 + b^2 // - static float DistanceSquared(ESM::Pathgrid::Point point, const osg::Vec3f& pos) + static float distanceSquared(ESM::Pathgrid::Point point, const osg::Vec3f& pos) { - return (MWMechanics::PathFinder::MakeOsgVec3(point) - pos).length2(); + return (MWMechanics::PathFinder::makeOsgVec3(point) - pos).length2(); } // Return the closest pathgrid point index from the specified position @@ -130,18 +150,18 @@ namespace MWMechanics // // NOTE: pos is expected to be in local coordinates, as is grid->mPoints // - static int GetClosestPoint(const ESM::Pathgrid* grid, const osg::Vec3f& pos) + static int getClosestPoint(const ESM::Pathgrid* grid, const osg::Vec3f& pos) { assert(grid && !grid->mPoints.empty()); - float distanceBetween = DistanceSquared(grid->mPoints[0], pos); + float distanceBetween = distanceSquared(grid->mPoints[0], pos); int closestIndex = 0; // TODO: if this full scan causes performance problems mapping pathgrid // points to a quadtree may help for(unsigned int counter = 1; counter < grid->mPoints.size(); counter++) { - float potentialDistBetween = DistanceSquared(grid->mPoints[counter], pos); + float potentialDistBetween = distanceSquared(grid->mPoints[counter], pos); if(potentialDistBetween < distanceBetween) { distanceBetween = potentialDistBetween; @@ -153,10 +173,17 @@ namespace MWMechanics } private: - std::list mPath; + bool mConstructed; + std::deque mPath; - const ESM::Pathgrid *mPathgrid; const MWWorld::CellStore* mCell; + + void buildPathByPathgridImpl(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const PathgridGraph& pathgridGraph, std::back_insert_iterator> out); + + void buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, + const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, + std::back_insert_iterator> out); }; } diff --git a/apps/openmw/mwmechanics/pathgrid.cpp b/apps/openmw/mwmechanics/pathgrid.cpp index 6b5db64eab..7bcdc8926c 100644 --- a/apps/openmw/mwmechanics/pathgrid.cpp +++ b/apps/openmw/mwmechanics/pathgrid.cpp @@ -257,10 +257,9 @@ namespace MWMechanics * pathgrid points form (currently they are converted to world * coordinates). Essentially trading speed w/ memory. */ - std::list PathgridGraph::aStarSearch(const int start, - const int goal) const + std::deque PathgridGraph::aStarSearch(const int start, const int goal) const { - std::list path; + std::deque path; if(!isPointConnected(start, goal)) { return path; // there is no path, return an empty path diff --git a/apps/openmw/mwmechanics/pathgrid.hpp b/apps/openmw/mwmechanics/pathgrid.hpp index 0c71c45617..6b67bb5073 100644 --- a/apps/openmw/mwmechanics/pathgrid.hpp +++ b/apps/openmw/mwmechanics/pathgrid.hpp @@ -1,7 +1,7 @@ #ifndef GAME_MWMECHANICS_PATHGRID_H #define GAME_MWMECHANICS_PATHGRID_H -#include +#include #include @@ -38,8 +38,8 @@ namespace MWMechanics // cells) coordinates // // NOTE: if start equals end an empty path is returned - std::list aStarSearch(const int start, - const int end) const; + std::deque aStarSearch(const int start, const int end) const; + private: const ESM::Cell *mCell; diff --git a/apps/openmw/mwphysics/actor.hpp b/apps/openmw/mwphysics/actor.hpp index 8ec94200fb..31dd22a221 100644 --- a/apps/openmw/mwphysics/actor.hpp +++ b/apps/openmw/mwphysics/actor.hpp @@ -3,7 +3,7 @@ #include -#include "../mwworld/ptr.hpp" +#include "ptrholder.hpp" #include #include @@ -22,30 +22,6 @@ namespace Resource namespace MWPhysics { - class PtrHolder - { - public: - virtual ~PtrHolder() {} - - void updatePtr(const MWWorld::Ptr& updated) - { - mPtr = updated; - } - - MWWorld::Ptr getPtr() - { - return mPtr; - } - - MWWorld::ConstPtr getPtr() const - { - return mPtr; - } - - protected: - MWWorld::Ptr mPtr; - }; - class Actor : public PtrHolder { public: diff --git a/apps/openmw/mwphysics/heightfield.cpp b/apps/openmw/mwphysics/heightfield.cpp new file mode 100644 index 0000000000..52aed9c072 --- /dev/null +++ b/apps/openmw/mwphysics/heightfield.cpp @@ -0,0 +1,54 @@ +#include "heightfield.hpp" + +#include + +#include +#include + +#include + +namespace MWPhysics +{ + HeightField::HeightField(const float* heights, int x, int y, float triSize, float sqrtVerts, float minH, float maxH, const osg::Object* holdObject) + { + mShape = new btHeightfieldTerrainShape( + sqrtVerts, sqrtVerts, heights, 1, + minH, maxH, 2, + PHY_FLOAT, false + ); + mShape->setUseDiamondSubdivision(true); + mShape->setLocalScaling(btVector3(triSize, triSize, 1)); + + btTransform transform(btQuaternion::getIdentity(), + btVector3((x+0.5f) * triSize * (sqrtVerts-1), + (y+0.5f) * triSize * (sqrtVerts-1), + (maxH+minH)*0.5f)); + + mCollisionObject = new btCollisionObject; + mCollisionObject->setCollisionShape(mShape); + mCollisionObject->setWorldTransform(transform); + + mHoldObject = holdObject; + } + + HeightField::~HeightField() + { + delete mCollisionObject; + delete mShape; + } + + btCollisionObject* HeightField::getCollisionObject() + { + return mCollisionObject; + } + + const btCollisionObject* HeightField::getCollisionObject() const + { + return mCollisionObject; + } + + const btHeightfieldTerrainShape* HeightField::getShape() const + { + return mShape; + } +} diff --git a/apps/openmw/mwphysics/heightfield.hpp b/apps/openmw/mwphysics/heightfield.hpp new file mode 100644 index 0000000000..f248186db3 --- /dev/null +++ b/apps/openmw/mwphysics/heightfield.hpp @@ -0,0 +1,36 @@ +#ifndef OPENMW_MWPHYSICS_HEIGHTFIELD_H +#define OPENMW_MWPHYSICS_HEIGHTFIELD_H + +#include + +class btCollisionObject; +class btHeightfieldTerrainShape; + +namespace osg +{ + class Object; +} + +namespace MWPhysics +{ + class HeightField + { + public: + HeightField(const float* heights, int x, int y, float triSize, float sqrtVerts, float minH, float maxH, const osg::Object* holdObject); + ~HeightField(); + + btCollisionObject* getCollisionObject(); + const btCollisionObject* getCollisionObject() const; + const btHeightfieldTerrainShape* getShape() const; + + private: + btHeightfieldTerrainShape* mShape; + btCollisionObject* mCollisionObject; + osg::ref_ptr mHoldObject; + + void operator=(const HeightField&); + HeightField(const HeightField&); + }; +} + +#endif diff --git a/apps/openmw/mwphysics/object.cpp b/apps/openmw/mwphysics/object.cpp new file mode 100644 index 0000000000..2f28668215 --- /dev/null +++ b/apps/openmw/mwphysics/object.cpp @@ -0,0 +1,130 @@ +#include "object.hpp" +#include "convert.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +namespace MWPhysics +{ + Object::Object(const MWWorld::Ptr& ptr, osg::ref_ptr shapeInstance) + : mShapeInstance(shapeInstance) + , mSolid(true) + { + mPtr = ptr; + + mCollisionObject.reset(new btCollisionObject); + mCollisionObject->setCollisionShape(shapeInstance->getCollisionShape()); + + mCollisionObject->setUserPointer(static_cast(this)); + + setScale(ptr.getCellRef().getScale()); + setRotation(toBullet(ptr.getRefData().getBaseNode()->getAttitude())); + const float* pos = ptr.getRefData().getPosition().pos; + setOrigin(btVector3(pos[0], pos[1], pos[2])); + } + + const Resource::BulletShapeInstance* Object::getShapeInstance() const + { + return mShapeInstance.get(); + } + + void Object::setScale(float scale) + { + mShapeInstance->setLocalScaling(btVector3(scale, scale, scale)); + } + + void Object::setRotation(const btQuaternion& quat) + { + mCollisionObject->getWorldTransform().setRotation(quat); + } + + void Object::setOrigin(const btVector3& vec) + { + mCollisionObject->getWorldTransform().setOrigin(vec); + } + + btCollisionObject* Object::getCollisionObject() + { + return mCollisionObject.get(); + } + + const btCollisionObject* Object::getCollisionObject() const + { + return mCollisionObject.get(); + } + + bool Object::isSolid() const + { + return mSolid; + } + + void Object::setSolid(bool solid) + { + mSolid = solid; + } + + bool Object::isAnimated() const + { + return !mShapeInstance->mAnimatedShapes.empty(); + } + + void Object::animateCollisionShapes(btCollisionWorld* collisionWorld) + { + if (mShapeInstance->mAnimatedShapes.empty()) + return; + + assert (mShapeInstance->getCollisionShape()->isCompound()); + + btCompoundShape* compound = static_cast(mShapeInstance->getCollisionShape()); + for (std::map::const_iterator it = mShapeInstance->mAnimatedShapes.begin(); it != mShapeInstance->mAnimatedShapes.end(); ++it) + { + int recIndex = it->first; + int shapeIndex = it->second; + + std::map::iterator nodePathFound = mRecIndexToNodePath.find(recIndex); + if (nodePathFound == mRecIndexToNodePath.end()) + { + NifOsg::FindGroupByRecIndex visitor(recIndex); + mPtr.getRefData().getBaseNode()->accept(visitor); + if (!visitor.mFound) + { + Log(Debug::Warning) << "Warning: animateCollisionShapes can't find node " << recIndex << " for " << mPtr.getCellRef().getRefId(); + + // Remove nonexistent nodes from animated shapes map and early out + mShapeInstance->mAnimatedShapes.erase(recIndex); + return; + } + osg::NodePath nodePath = visitor.mFoundPath; + nodePath.erase(nodePath.begin()); + nodePathFound = mRecIndexToNodePath.insert(std::make_pair(recIndex, nodePath)).first; + } + + osg::NodePath& nodePath = nodePathFound->second; + osg::Matrixf matrix = osg::computeLocalToWorld(nodePath); + matrix.orthoNormalize(matrix); + + btTransform transform; + transform.setOrigin(toBullet(matrix.getTrans()) * compound->getLocalScaling()); + for (int i=0; i<3; ++i) + for (int j=0; j<3; ++j) + transform.getBasis()[i][j] = matrix(j,i); // NB column/row major difference + + // Note: we can not apply scaling here for now since we treat scaled shapes + // as new shapes (btScaledBvhTriangleMeshShape) with 1.0 scale for now + if (!(transform == compound->getChildTransform(shapeIndex))) + compound->updateChildTransform(shapeIndex, transform); + } + + collisionWorld->updateSingleAabb(mCollisionObject.get()); + } +} diff --git a/apps/openmw/mwphysics/object.hpp b/apps/openmw/mwphysics/object.hpp new file mode 100644 index 0000000000..b948433e36 --- /dev/null +++ b/apps/openmw/mwphysics/object.hpp @@ -0,0 +1,48 @@ +#ifndef OPENMW_MWPHYSICS_OBJECT_H +#define OPENMW_MWPHYSICS_OBJECT_H + +#include "ptrholder.hpp" + +#include + +#include +#include + +namespace Resource +{ + class BulletShapeInstance; +} + +class btCollisionObject; +class btCollisionWorld; +class btQuaternion; +class btVector3; + +namespace MWPhysics +{ + class Object : public PtrHolder + { + public: + Object(const MWWorld::Ptr& ptr, osg::ref_ptr shapeInstance); + + const Resource::BulletShapeInstance* getShapeInstance() const; + void setScale(float scale); + void setRotation(const btQuaternion& quat); + void setOrigin(const btVector3& vec); + btCollisionObject* getCollisionObject(); + const btCollisionObject* getCollisionObject() const; + /// Return solid flag. Not used by the object itself, true by default. + bool isSolid() const; + void setSolid(bool solid); + bool isAnimated() const; + void animateCollisionShapes(btCollisionWorld* collisionWorld); + + private: + std::unique_ptr mCollisionObject; + osg::ref_ptr mShapeInstance; + std::map mRecIndexToNodePath; + bool mSolid; + }; +} + +#endif diff --git a/apps/openmw/mwphysics/physicssystem.cpp b/apps/openmw/mwphysics/physicssystem.cpp index 9ee47f3f5c..57b6d348bd 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 @@ -48,12 +59,12 @@ #include "actor.hpp" #include "convert.hpp" #include "trace.h" +#include "object.hpp" +#include "heightfield.hpp" 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; @@ -507,175 +518,6 @@ namespace MWPhysics // --------------------------------------------------------------- - class HeightField - { - public: - HeightField(const float* heights, int x, int y, float triSize, float sqrtVerts, float minH, float maxH, const osg::Object* holdObject) - { - mShape = new btHeightfieldTerrainShape( - sqrtVerts, sqrtVerts, heights, 1, - minH, maxH, 2, - PHY_FLOAT, false - ); - mShape->setUseDiamondSubdivision(true); - mShape->setLocalScaling(btVector3(triSize, triSize, 1)); - - btTransform transform(btQuaternion::getIdentity(), - btVector3((x+0.5f) * triSize * (sqrtVerts-1), - (y+0.5f) * triSize * (sqrtVerts-1), - (maxH+minH)*0.5f)); - - mCollisionObject = new btCollisionObject; - mCollisionObject->setCollisionShape(mShape); - mCollisionObject->setWorldTransform(transform); - - mHoldObject = holdObject; - } - ~HeightField() - { - delete mCollisionObject; - delete mShape; - } - btCollisionObject* getCollisionObject() - { - return mCollisionObject; - } - - private: - btHeightfieldTerrainShape* mShape; - btCollisionObject* mCollisionObject; - osg::ref_ptr mHoldObject; - - void operator=(const HeightField&); - HeightField(const HeightField&); - }; - - // -------------------------------------------------------------- - - class Object : public PtrHolder - { - public: - Object(const MWWorld::Ptr& ptr, osg::ref_ptr shapeInstance) - : mShapeInstance(shapeInstance) - , mSolid(true) - { - mPtr = ptr; - - mCollisionObject.reset(new btCollisionObject); - mCollisionObject->setCollisionShape(shapeInstance->getCollisionShape()); - - mCollisionObject->setUserPointer(static_cast(this)); - - setScale(ptr.getCellRef().getScale()); - setRotation(toBullet(ptr.getRefData().getBaseNode()->getAttitude())); - const float* pos = ptr.getRefData().getPosition().pos; - setOrigin(btVector3(pos[0], pos[1], pos[2])); - } - - const Resource::BulletShapeInstance* getShapeInstance() const - { - return mShapeInstance.get(); - } - - void setScale(float scale) - { - mShapeInstance->getCollisionShape()->setLocalScaling(btVector3(scale,scale,scale)); - } - - void setRotation(const btQuaternion& quat) - { - mCollisionObject->getWorldTransform().setRotation(quat); - } - - void setOrigin(const btVector3& vec) - { - mCollisionObject->getWorldTransform().setOrigin(vec); - } - - btCollisionObject* getCollisionObject() - { - return mCollisionObject.get(); - } - - const btCollisionObject* getCollisionObject() const - { - return mCollisionObject.get(); - } - - /// Return solid flag. Not used by the object itself, true by default. - bool isSolid() const - { - return mSolid; - } - - void setSolid(bool solid) - { - mSolid = solid; - } - - bool isAnimated() const - { - return !mShapeInstance->mAnimatedShapes.empty(); - } - - void animateCollisionShapes(btCollisionWorld* collisionWorld) - { - if (mShapeInstance->mAnimatedShapes.empty()) - return; - - assert (mShapeInstance->getCollisionShape()->isCompound()); - - btCompoundShape* compound = static_cast(mShapeInstance->getCollisionShape()); - for (std::map::const_iterator it = mShapeInstance->mAnimatedShapes.begin(); it != mShapeInstance->mAnimatedShapes.end(); ++it) - { - int recIndex = it->first; - int shapeIndex = it->second; - - std::map::iterator nodePathFound = mRecIndexToNodePath.find(recIndex); - if (nodePathFound == mRecIndexToNodePath.end()) - { - NifOsg::FindGroupByRecIndex visitor(recIndex); - mPtr.getRefData().getBaseNode()->accept(visitor); - if (!visitor.mFound) - { - Log(Debug::Warning) << "Warning: animateCollisionShapes can't find node " << recIndex << " for " << mPtr.getCellRef().getRefId(); - - // Remove nonexistent nodes from animated shapes map and early out - mShapeInstance->mAnimatedShapes.erase(recIndex); - return; - } - osg::NodePath nodePath = visitor.mFoundPath; - nodePath.erase(nodePath.begin()); - nodePathFound = mRecIndexToNodePath.insert(std::make_pair(recIndex, nodePath)).first; - } - - osg::NodePath& nodePath = nodePathFound->second; - osg::Matrixf matrix = osg::computeLocalToWorld(nodePath); - matrix.orthoNormalize(matrix); - - btTransform transform; - transform.setOrigin(toBullet(matrix.getTrans()) * compound->getLocalScaling()); - for (int i=0; i<3; ++i) - for (int j=0; j<3; ++j) - transform.getBasis()[i][j] = matrix(j,i); // NB column/row major difference - - // Note: we can not apply scaling here for now since we treat scaled shapes - // as new shapes (btScaledBvhTriangleMeshShape) with 1.0 scale for now - if (!(transform == compound->getChildTransform(shapeIndex))) - compound->updateChildTransform(shapeIndex, transform); - } - - collisionWorld->updateSingleAabb(mCollisionObject.get()); - } - - private: - std::unique_ptr mCollisionObject; - osg::ref_ptr mShapeInstance; - std::map mRecIndexToNodePath; - bool mSolid; - }; - - // --------------------------------------------------------------- PhysicsSystem::PhysicsSystem(Resource::ResourceSystem* resourceSystem, osg::ref_ptr parentNode) : mShapeManager(new Resource::BulletShapeManager(resourceSystem->getVFS(), resourceSystem->getSceneManager(), resourceSystem->getNifFileManager())) @@ -1171,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 2de869395d..d4829f6232 100644 --- a/apps/openmw/mwphysics/physicssystem.hpp +++ b/apps/openmw/mwphysics/physicssystem.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -49,6 +50,9 @@ namespace MWPhysics class Object; class Actor; + static const float sMaxSlope = 49.0f; + static const float sStepSizeUp = 34.0f; + class PhysicsSystem { public: @@ -85,6 +89,8 @@ namespace MWPhysics void removeHeightField (int x, int y); + const HeightField* getHeightField(int x, int y) const; + bool toggleCollisionMode(); bool isActorCollisionEnabled(const MWWorld::Ptr& ptr); void setActorCollisionMode(const MWWorld::Ptr& ptr, bool enabled); @@ -172,6 +178,12 @@ namespace MWPhysics bool isOnSolidGround (const MWWorld::Ptr& actor) const; + template + void forEachAnimatedObject(Function&& function) const + { + std::for_each(mAnimatedObjects.begin(), mAnimatedObjects.end(), function); + } + private: void updateWater(); diff --git a/apps/openmw/mwphysics/ptrholder.hpp b/apps/openmw/mwphysics/ptrholder.hpp new file mode 100644 index 0000000000..f8188b43ed --- /dev/null +++ b/apps/openmw/mwphysics/ptrholder.hpp @@ -0,0 +1,33 @@ +#ifndef OPENMW_MWPHYSICS_PTRHOLDER_H +#define OPENMW_MWPHYSICS_PTRHOLDER_H + +#include "../mwworld/ptr.hpp" + +namespace MWPhysics +{ + class PtrHolder + { + public: + virtual ~PtrHolder() {} + + void updatePtr(const MWWorld::Ptr& updated) + { + mPtr = updated; + } + + MWWorld::Ptr getPtr() + { + return mPtr; + } + + MWWorld::ConstPtr getPtr() const + { + return mPtr; + } + + protected: + MWWorld::Ptr mPtr; + }; +} + +#endif diff --git a/apps/openmw/mwrender/actorspaths.cpp b/apps/openmw/mwrender/actorspaths.cpp new file mode 100644 index 0000000000..35b2553555 --- /dev/null +++ b/apps/openmw/mwrender/actorspaths.cpp @@ -0,0 +1,99 @@ +#include "actorspaths.hpp" +#include "vismask.hpp" + +#include + +#include + +namespace MWRender +{ + ActorsPaths::ActorsPaths(const osg::ref_ptr& root, bool enabled) + : mRootNode(root) + , mEnabled(enabled) + { + } + + ActorsPaths::~ActorsPaths() + { + if (mEnabled) + disable(); + } + + bool ActorsPaths::toggle() + { + if (mEnabled) + disable(); + else + enable(); + + return mEnabled; + } + + void ActorsPaths::update(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end, + const DetourNavigator::Settings& settings) + { + if (!mEnabled) + return; + + const auto group = mGroups.find(actor); + if (group != mGroups.end()) + mRootNode->removeChild(group->second); + + const auto newGroup = SceneUtil::createAgentPathGroup(path, halfExtents, start, end, settings); + if (newGroup) + { + newGroup->setNodeMask(Mask_Debug); + mRootNode->addChild(newGroup); + mGroups[actor] = newGroup; + } + } + + void ActorsPaths::remove(const MWWorld::ConstPtr& actor) + { + const auto group = mGroups.find(actor); + if (group != mGroups.end()) + { + mRootNode->removeChild(group->second); + mGroups.erase(group); + } + } + + void ActorsPaths::removeCell(const MWWorld::CellStore* const store) + { + for (auto it = mGroups.begin(); it != mGroups.end(); ) + { + if (it->first.getCell() == store) + { + mRootNode->removeChild(it->second); + it = mGroups.erase(it); + } + else + ++it; + } + } + + void ActorsPaths::updatePtr(const MWWorld::ConstPtr& old, const MWWorld::ConstPtr& updated) + { + const auto it = mGroups.find(old); + if (it == mGroups.end()) + return; + auto group = std::move(it->second); + mGroups.erase(it); + mGroups.insert(std::make_pair(updated, std::move(group))); + } + + void ActorsPaths::enable() + { + std::for_each(mGroups.begin(), mGroups.end(), + [&] (const Groups::value_type& v) { mRootNode->addChild(v.second); }); + mEnabled = true; + } + + void ActorsPaths::disable() + { + std::for_each(mGroups.begin(), mGroups.end(), + [&] (const Groups::value_type& v) { mRootNode->removeChild(v.second); }); + mEnabled = false; + } +} diff --git a/apps/openmw/mwrender/actorspaths.hpp b/apps/openmw/mwrender/actorspaths.hpp new file mode 100644 index 0000000000..1f61834d46 --- /dev/null +++ b/apps/openmw/mwrender/actorspaths.hpp @@ -0,0 +1,51 @@ +#ifndef OPENMW_MWRENDER_AGENTSPATHS_H +#define OPENMW_MWRENDER_AGENTSPATHS_H + +#include + +#include + +#include + +#include +#include + +namespace osg +{ + class Group; +} + +namespace MWRender +{ + class ActorsPaths + { + public: + ActorsPaths(const osg::ref_ptr& root, bool enabled); + ~ActorsPaths(); + + bool toggle(); + + void update(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end, + const DetourNavigator::Settings& settings); + + void remove(const MWWorld::ConstPtr& actor); + + void removeCell(const MWWorld::CellStore* const store); + + void updatePtr(const MWWorld::ConstPtr& old, const MWWorld::ConstPtr& updated); + + void enable(); + + void disable(); + + private: + using Groups = std::map>; + + osg::ref_ptr mRootNode; + Groups mGroups; + bool mEnabled; + }; +} + +#endif diff --git a/apps/openmw/mwrender/navmesh.cpp b/apps/openmw/mwrender/navmesh.cpp new file mode 100644 index 0000000000..331f506abb --- /dev/null +++ b/apps/openmw/mwrender/navmesh.cpp @@ -0,0 +1,71 @@ +#include "navmesh.hpp" +#include "vismask.hpp" + +#include + +#include + +namespace MWRender +{ + NavMesh::NavMesh(const osg::ref_ptr& root, bool enabled) + : mRootNode(root) + , mEnabled(enabled) + , mGeneration(0) + , mRevision(0) + { + } + + NavMesh::~NavMesh() + { + if (mEnabled) + disable(); + } + + bool NavMesh::toggle() + { + if (mEnabled) + disable(); + else + enable(); + + return mEnabled; + } + + void NavMesh::update(const dtNavMesh& navMesh, const std::size_t id, + const std::size_t generation, const std::size_t revision, const DetourNavigator::Settings& settings) + { + if (!mEnabled || (mId == id && mGeneration >= generation && mRevision >= revision)) + return; + + mId = id; + mGeneration = generation; + mRevision = revision; + if (mGroup) + mRootNode->removeChild(mGroup); + mGroup = SceneUtil::createNavMeshGroup(navMesh, settings); + if (mGroup) + { + mGroup->setNodeMask(Mask_Debug); + mRootNode->addChild(mGroup); + } + } + + void NavMesh::reset() + { + if (mGroup) + mRootNode->removeChild(mGroup); + } + + void NavMesh::enable() + { + if (mGroup) + mRootNode->addChild(mGroup); + mEnabled = true; + } + + void NavMesh::disable() + { + reset(); + mEnabled = false; + } +} diff --git a/apps/openmw/mwrender/navmesh.hpp b/apps/openmw/mwrender/navmesh.hpp new file mode 100644 index 0000000000..29205ca277 --- /dev/null +++ b/apps/openmw/mwrender/navmesh.hpp @@ -0,0 +1,43 @@ +#ifndef OPENMW_MWRENDER_NAVMESH_H +#define OPENMW_MWRENDER_NAVMESH_H + +#include + +#include + +namespace osg +{ + class Group; + class Geometry; +} + +namespace MWRender +{ + class NavMesh + { + public: + NavMesh(const osg::ref_ptr& root, bool enabled); + ~NavMesh(); + + bool toggle(); + + void update(const dtNavMesh& navMesh, const std::size_t number, const std::size_t generation, + const std::size_t revision, const DetourNavigator::Settings& settings); + + void reset(); + + void enable(); + + void disable(); + + private: + osg::ref_ptr mRootNode; + bool mEnabled; + std::size_t mId = std::numeric_limits::max(); + std::size_t mGeneration; + std::size_t mRevision; + osg::ref_ptr mGroup; + }; +} + +#endif diff --git a/apps/openmw/mwrender/renderingmanager.cpp b/apps/openmw/mwrender/renderingmanager.cpp index 476beb990b..4d79fd3de2 100644 --- a/apps/openmw/mwrender/renderingmanager.cpp +++ b/apps/openmw/mwrender/renderingmanager.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include #include "../mwworld/cellstore.hpp" @@ -63,6 +65,8 @@ #include "water.hpp" #include "terrainstorage.hpp" #include "util.hpp" +#include "navmesh.hpp" +#include "actorspaths.hpp" namespace { @@ -189,13 +193,16 @@ namespace MWRender Resource::ResourceSystem* mResourceSystem; }; - RenderingManager::RenderingManager(osgViewer::Viewer* viewer, osg::ref_ptr rootNode, Resource::ResourceSystem* resourceSystem, SceneUtil::WorkQueue* workQueue, - const Fallback::Map* fallback, const std::string& resourcePath) + RenderingManager::RenderingManager(osgViewer::Viewer* viewer, osg::ref_ptr rootNode, + Resource::ResourceSystem* resourceSystem, SceneUtil::WorkQueue* workQueue, + const Fallback::Map* fallback, const std::string& resourcePath, + DetourNavigator::Navigator& navigator) : mViewer(viewer) , mRootNode(rootNode) , mResourceSystem(resourceSystem) , mWorkQueue(workQueue) , mUnrefQueue(new SceneUtil::UnrefQueue) + , mNavigator(navigator) , mLandFogStart(0.f) , mLandFogEnd(std::numeric_limits::max()) , mUnderwaterFogStart(0.f) @@ -228,6 +235,8 @@ namespace MWRender mRootNode->addChild(mSceneRoot); + mNavMesh.reset(new NavMesh(mRootNode, Settings::Manager::getBool("enable nav mesh render", "Navigator"))); + mActorsPaths.reset(new ActorsPaths(mRootNode, Settings::Manager::getBool("enable agents paths render", "Navigator"))); mPathgrid.reset(new Pathgrid(mRootNode)); mObjects.reset(new Objects(mResourceSystem, sceneRoot, mUnrefQueue.get())); @@ -465,6 +474,7 @@ namespace MWRender void RenderingManager::removeCell(const MWWorld::CellStore *store) { mPathgrid->removeCell(store); + mActorsPaths->removeCell(store); mObjects->removeCell(store); if (store->getCell()->isExterior()) @@ -482,7 +492,7 @@ namespace MWRender { mSky->setEnabled(enabled); } - + bool RenderingManager::toggleBorders() { mBorders = !mBorders; @@ -516,6 +526,14 @@ namespace MWRender mViewer->getCamera()->setCullMask(mask); return enabled; } + else if (mode == Render_NavMesh) + { + return mNavMesh->toggle(); + } + else if (mode == Render_ActorsPaths) + { + return mActorsPaths->toggle(); + } return false; } @@ -581,6 +599,28 @@ namespace MWRender mWater->update(dt); } + const auto navMeshes = mNavigator.getNavMeshes(); + + auto it = navMeshes.begin(); + for (std::size_t i = 0; it != navMeshes.end() && i < mNavMeshNumber; ++i) + ++it; + if (it == navMeshes.end()) + { + mNavMesh->reset(); + } + else + { + try + { + const auto locked = it->second.lockConst(); + mNavMesh->update(locked->getValue(), mNavMeshNumber, locked->getGeneration(), + locked->getNavMeshRevision(), mNavigator.getSettings()); + } + catch (const std::exception& e) + { + Log(Debug::Error) << "NavMesh render update exception: " << e.what(); + } + } mCamera->update(dt, paused); osg::Vec3f focal, cameraPos; @@ -642,6 +682,7 @@ namespace MWRender void RenderingManager::removeObject(const MWWorld::Ptr &ptr) { + mActorsPaths->remove(ptr); mObjects->removeObject(ptr); mWater->removeEmitter(ptr); } @@ -749,8 +790,8 @@ namespace MWRender osg::Vec3 directions[6] = { rawCubemap ? osg::Vec3(1,0,0) : osg::Vec3(0,0,1), - osg::Vec3(0,0,-1), - osg::Vec3(-1,0,0), + osg::Vec3(0,0,-1), + osg::Vec3(-1,0,0), rawCubemap ? osg::Vec3(0,0,1) : osg::Vec3(1,0,0), osg::Vec3(0,1,0), osg::Vec3(0,-1,0)}; @@ -789,7 +830,7 @@ namespace MWRender mFieldOfView = fovBackup; if (rawCubemap) // for raw cubemap don't run on GPU, just merge the images - { + { image->allocateImage(cubeSize * 6,cubeSize,images[0]->r(),images[0]->getPixelFormat(),images[0]->getDataType()); for (int i = 0; i < 6; ++i) @@ -797,7 +838,7 @@ namespace MWRender return true; } - + // run on GPU now: osg::ref_ptr cubeTexture (new osg::TextureCubeMap); @@ -1025,6 +1066,7 @@ namespace MWRender void RenderingManager::updatePtr(const MWWorld::Ptr &old, const MWWorld::Ptr &updated) { mObjects->updatePtr(old, updated); + mActorsPaths->updatePtr(old, updated); } void RenderingManager::spawnEffect(const std::string &model, const std::string &texture, const osg::Vec3f &worldPosition, float scale, bool isMagicVFX) @@ -1345,5 +1387,19 @@ namespace MWRender return mTerrainStorage->getLandManager(); } + void RenderingManager::updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const + { + mActorsPaths->update(actor, path, halfExtents, start, end, mNavigator.getSettings()); + } + void RenderingManager::removeActorPath(const MWWorld::ConstPtr& actor) const + { + mActorsPaths->remove(actor); + } + + void RenderingManager::setNavMeshNumber(const std::size_t value) + { + mNavMeshNumber = value; + } } diff --git a/apps/openmw/mwrender/renderingmanager.hpp b/apps/openmw/mwrender/renderingmanager.hpp index b4473c3b4f..7a4500ac90 100644 --- a/apps/openmw/mwrender/renderingmanager.hpp +++ b/apps/openmw/mwrender/renderingmanager.hpp @@ -12,6 +12,8 @@ #include "renderinginterface.hpp" #include "rendermode.hpp" +#include + namespace osg { class Group; @@ -55,6 +57,12 @@ namespace SceneUtil class UnrefQueue; } +namespace DetourNavigator +{ + class Navigator; + struct Settings; +} + namespace MWRender { @@ -68,12 +76,16 @@ namespace MWRender class Water; class TerrainStorage; class LandManager; + class NavMesh; + class ActorsPaths; class RenderingManager : public MWRender::RenderingInterface { public: - RenderingManager(osgViewer::Viewer* viewer, osg::ref_ptr rootNode, Resource::ResourceSystem* resourceSystem, SceneUtil::WorkQueue* workQueue, - const Fallback::Map* fallback, const std::string& resourcePath); + RenderingManager(osgViewer::Viewer* viewer, osg::ref_ptr rootNode, + Resource::ResourceSystem* resourceSystem, SceneUtil::WorkQueue* workQueue, + const Fallback::Map* fallback, const std::string& resourcePath, + DetourNavigator::Navigator& navigator); ~RenderingManager(); MWRender::Objects& getObjects(); @@ -211,6 +223,13 @@ namespace MWRender bool toggleBorders(); + void updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const; + + void removeActorPath(const MWWorld::ConstPtr& actor) const; + + void setNavMeshNumber(const std::size_t value); + private: void updateProjectionMatrix(); void updateTextureFiltering(); @@ -235,6 +254,10 @@ namespace MWRender osg::ref_ptr mSunLight; + DetourNavigator::Navigator& mNavigator; + std::unique_ptr mNavMesh; + std::size_t mNavMeshNumber = 0; + std::unique_ptr mActorsPaths; std::unique_ptr mPathgrid; std::unique_ptr mObjects; std::unique_ptr mWater; diff --git a/apps/openmw/mwrender/rendermode.hpp b/apps/openmw/mwrender/rendermode.hpp index 2847888d1e..077710f4f4 100644 --- a/apps/openmw/mwrender/rendermode.hpp +++ b/apps/openmw/mwrender/rendermode.hpp @@ -10,7 +10,9 @@ namespace MWRender Render_Wireframe, Render_Pathgrid, Render_Water, - Render_Scene + Render_Scene, + Render_NavMesh, + Render_ActorsPaths, }; } diff --git a/apps/openmw/mwscript/docs/vmformat.txt b/apps/openmw/mwscript/docs/vmformat.txt index fed780be72..d346ab6e43 100644 --- a/apps/openmw/mwscript/docs/vmformat.txt +++ b/apps/openmw/mwscript/docs/vmformat.txt @@ -455,5 +455,8 @@ op 0x2000304: Show op 0x2000305: Show, explicit op 0x2000306: OnActivate, explicit op 0x2000307: ToggleBorders, tb +op 0x2000308: ToggleNavMesh +op 0x2000309: ToggleActorsPaths +op 0x200030a: SetNavMeshNumber -opcodes 0x2000308-0x3ffffff unused +opcodes 0x200030b-0x3ffffff unused diff --git a/apps/openmw/mwscript/miscextensions.cpp b/apps/openmw/mwscript/miscextensions.cpp index 7250e9fcfc..0cf2fc464b 100644 --- a/apps/openmw/mwscript/miscextensions.cpp +++ b/apps/openmw/mwscript/miscextensions.cpp @@ -1317,6 +1317,53 @@ namespace MWScript } }; + class OpToggleNavMesh : public Interpreter::Opcode0 + { + public: + + virtual void execute (Interpreter::Runtime& runtime) + { + bool enabled = + MWBase::Environment::get().getWorld()->toggleRenderMode (MWRender::Render_NavMesh); + + runtime.getContext().report (enabled ? + "Navigation Mesh Rendering -> On" : "Navigation Mesh Rendering -> Off"); + } + }; + + class OpToggleActorsPaths : public Interpreter::Opcode0 + { + public: + + virtual void execute (Interpreter::Runtime& runtime) + { + bool enabled = + MWBase::Environment::get().getWorld()->toggleRenderMode (MWRender::Render_ActorsPaths); + + runtime.getContext().report (enabled ? + "Agents Paths Rendering -> On" : "Agents Paths Rendering -> Off"); + } + }; + + class OpSetNavMeshNumberToRender : public Interpreter::Opcode0 + { + public: + + virtual void execute (Interpreter::Runtime& runtime) + { + const auto navMeshNumber = runtime[0].mInteger; + runtime.pop(); + + if (navMeshNumber < 0) + { + runtime.getContext().report("Invalid navmesh number: use not less than zero values"); + return; + } + + MWBase::Environment::get().getWorld()->setNavMeshNumberToRender(static_cast(navMeshNumber)); + } + }; + void installOpcodes (Interpreter::Interpreter& interpreter) { interpreter.installSegment5 (Compiler::Misc::opcodeXBox, new OpXBox); @@ -1417,6 +1464,9 @@ namespace MWScript interpreter.installSegment3 (Compiler::Misc::opcodeShowSceneGraph, new OpShowSceneGraph); interpreter.installSegment3 (Compiler::Misc::opcodeShowSceneGraphExplicit, new OpShowSceneGraph); interpreter.installSegment5 (Compiler::Misc::opcodeToggleBorders, new OpToggleBorders); + interpreter.installSegment5 (Compiler::Misc::opcodeToggleNavMesh, new OpToggleNavMesh); + interpreter.installSegment5 (Compiler::Misc::opcodeToggleActorsPaths, new OpToggleActorsPaths); + interpreter.installSegment5 (Compiler::Misc::opcodeSetNavMeshNumberToRender, new OpSetNavMeshNumberToRender); } } } diff --git a/apps/openmw/mwworld/cellstore.cpp b/apps/openmw/mwworld/cellstore.cpp index 239d714a06..520630bac9 100644 --- a/apps/openmw/mwworld/cellstore.cpp +++ b/apps/openmw/mwworld/cellstore.cpp @@ -432,7 +432,7 @@ namespace MWWorld mHasState = true; } - int CellStore::count() const + std::size_t CellStore::count() const { return mMergedRefs.size(); } diff --git a/apps/openmw/mwworld/cellstore.hpp b/apps/openmw/mwworld/cellstore.hpp index 1c4d8f5d87..49bc0b3639 100644 --- a/apps/openmw/mwworld/cellstore.hpp +++ b/apps/openmw/mwworld/cellstore.hpp @@ -240,7 +240,7 @@ namespace MWWorld ESM::FogState* getFog () const; - int count() const; + std::size_t count() const; ///< Return total number of references, including deleted ones. void load (); @@ -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 4c6cf1962b..1d5293b7ce 100644 --- a/apps/openmw/mwworld/scene.cpp +++ b/apps/openmw/mwworld/scene.cpp @@ -2,12 +2,19 @@ #include +#include +#include +#include + #include #include #include #include #include #include +#include +#include +#include #include "../mwbase/environment.hpp" #include "../mwbase/world.hpp" @@ -19,6 +26,10 @@ #include "../mwrender/landmanager.hpp" #include "../mwphysics/physicssystem.hpp" +#include "../mwphysics/actor.hpp" +#include "../mwphysics/object.hpp" +#include "../mwphysics/heightfield.hpp" +#include "../mwphysics/convert.hpp" #include "player.hpp" #include "localscripts.hpp" @@ -30,25 +41,45 @@ namespace { + osg::Quat makeActorOsgQuat(const ESM::Position& position) + { + return osg::Quat(position.rot[2], osg::Vec3(0, 0, -1)); + } - void setNodeRotation(const MWWorld::Ptr& ptr, MWRender::RenderingManager& rendering, bool inverseRotationOrder) + osg::Quat makeInversedOrderObjectOsgQuat(const ESM::Position& position) + { + const float xr = position.rot[0]; + const float yr = position.rot[1]; + const float zr = position.rot[2]; + + return osg::Quat(xr, osg::Vec3(-1, 0, 0)) + * osg::Quat(yr, osg::Vec3(0, -1, 0)) + * osg::Quat(zr, osg::Vec3(0, 0, -1)); + } + + osg::Quat makeObjectOsgQuat(const ESM::Position& position) + { + const float xr = position.rot[0]; + const float yr = position.rot[1]; + const float zr = position.rot[2]; + + return osg::Quat(zr, osg::Vec3(0, 0, -1)) + * osg::Quat(yr, osg::Vec3(0, -1, 0)) + * osg::Quat(xr, osg::Vec3(-1, 0, 0)); + } + + void setNodeRotation(const MWWorld::Ptr& ptr, MWRender::RenderingManager& rendering, const bool inverseRotationOrder) { if (!ptr.getRefData().getBaseNode()) return; - osg::Quat worldRotQuat(ptr.getRefData().getPosition().rot[2], osg::Vec3(0,0,-1)); - if (!ptr.getClass().isActor()) - { - float xr = ptr.getRefData().getPosition().rot[0]; - float yr = ptr.getRefData().getPosition().rot[1]; - if (!inverseRotationOrder) - worldRotQuat = worldRotQuat * osg::Quat(yr, osg::Vec3(0,-1,0)) * - osg::Quat(xr, osg::Vec3(-1,0,0)); - else - worldRotQuat = osg::Quat(xr, osg::Vec3(-1,0,0)) * osg::Quat(yr, osg::Vec3(0,-1,0)) * worldRotQuat; - } - - rendering.rotateObject(ptr, worldRotQuat); + rendering.rotateObject(ptr, + ptr.getClass().isActor() + ? makeActorOsgQuat(ptr.getRefData().getPosition()) + : (inverseRotationOrder + ? makeInversedOrderObjectOsgQuat(ptr.getRefData().getPosition()) + : makeObjectOsgQuat(ptr.getRefData().getPosition())) + ); } void addObject(const MWWorld::Ptr& ptr, MWPhysics::PhysicsSystem& physics, @@ -84,6 +115,73 @@ namespace MWBase::Environment::get().getWorld()->applyLoopingParticles(ptr); } + void addObject(const MWWorld::Ptr& ptr, const MWPhysics::PhysicsSystem& physics, DetourNavigator::Navigator& navigator) + { + if (const auto object = physics.getObject(ptr)) + { + if (ptr.getClass().isDoor() && !ptr.getCellRef().getTeleport()) + { + const auto shape = object->getShapeInstance()->getCollisionShape(); + + btVector3 aabbMin; + btVector3 aabbMax; + shape->getAabb(btTransform::getIdentity(), aabbMin, aabbMax); + + const auto center = (aabbMax + aabbMin) * 0.5f; + + const auto distanceFromDoor = MWBase::Environment::get().getWorld()->getMaxActivationDistance() * 0.5f; + const auto toPoint = aabbMax.x() - aabbMin.x() < aabbMax.y() - aabbMin.y() + ? btVector3(distanceFromDoor, 0, 0) + : btVector3(0, distanceFromDoor, 0); + + const auto& transform = object->getCollisionObject()->getWorldTransform(); + const btTransform closedDoorTransform( + MWPhysics::toBullet(makeObjectOsgQuat(ptr.getCellRef().getPosition())), + transform.getOrigin() + ); + + const auto start = DetourNavigator::makeOsgVec3f(closedDoorTransform(center + toPoint)); + const auto startPoint = physics.castRay(start, start - osg::Vec3f(0, 0, 1000), ptr, {}, + MWPhysics::CollisionType_World | MWPhysics::CollisionType_HeightMap | MWPhysics::CollisionType_Water); + const auto connectionStart = startPoint.mHit ? startPoint.mHitPos : start; + + const auto end = DetourNavigator::makeOsgVec3f(closedDoorTransform(center - toPoint)); + const auto endPoint = physics.castRay(end, end - osg::Vec3f(0, 0, 1000), ptr, {}, + MWPhysics::CollisionType_World | MWPhysics::CollisionType_HeightMap | MWPhysics::CollisionType_Water); + const auto connectionEnd = endPoint.mHit ? endPoint.mHitPos : end; + + navigator.addObject( + DetourNavigator::ObjectId(object), + DetourNavigator::DoorShapes( + *shape, + object->getShapeInstance()->getAvoidCollisionShape(), + connectionStart, + connectionEnd + ), + transform + ); + } + else + { + navigator.addObject( + DetourNavigator::ObjectId(object), + DetourNavigator::ObjectShapes { + *object->getShapeInstance()->getCollisionShape(), + object->getShapeInstance()->getAvoidCollisionShape() + }, + object->getCollisionObject()->getWorldTransform() + ); + } + } + else if (const auto actor = physics.getActor(ptr)) + { + const auto halfExtents = ptr.getCell()->isExterior() + ? physics.getHalfExtents(MWBase::Environment::get().getWorld()->getPlayerPtr()) + : actor->getHalfExtents(); + navigator.addAgent(halfExtents); + } + } + void updateObjectRotation (const MWWorld::Ptr& ptr, MWPhysics::PhysicsSystem& physics, MWRender::RenderingManager& rendering, bool inverseRotationOrder) { @@ -110,24 +208,19 @@ namespace MWWorld::CellStore& mCell; bool mRescale; Loading::Listener& mLoadingListener; - MWPhysics::PhysicsSystem& mPhysics; - MWRender::RenderingManager& mRendering; std::vector mToInsert; - InsertVisitor (MWWorld::CellStore& cell, bool rescale, Loading::Listener& loadingListener, - MWPhysics::PhysicsSystem& physics, MWRender::RenderingManager& rendering); + InsertVisitor (MWWorld::CellStore& cell, bool rescale, Loading::Listener& loadingListener); bool operator() (const MWWorld::Ptr& ptr); - void insert(); + + template + void insert(AddObject&& addObject); }; - InsertVisitor::InsertVisitor (MWWorld::CellStore& cell, bool rescale, - Loading::Listener& loadingListener, MWPhysics::PhysicsSystem& physics, - MWRender::RenderingManager& rendering) - : mCell (cell), mRescale (rescale), mLoadingListener (loadingListener), - mPhysics (physics), - mRendering (rendering) + InsertVisitor::InsertVisitor (MWWorld::CellStore& cell, bool rescale, Loading::Listener& loadingListener) + : mCell (cell), mRescale (rescale), mLoadingListener (loadingListener) {} bool InsertVisitor::operator() (const MWWorld::Ptr& ptr) @@ -138,7 +231,8 @@ namespace return true; } - void InsertVisitor::insert() + template + void InsertVisitor::insert(AddObject&& addObject) { for (std::vector::iterator it = mToInsert.begin(); it != mToInsert.end(); ++it) { @@ -155,7 +249,7 @@ namespace { try { - addObject(ptr, mPhysics, mRendering); + addObject(ptr); } catch (const std::exception& e) { @@ -178,6 +272,11 @@ namespace } }; + int getCellPositionDistanceToOrigin(const std::pair& cellPosition) + { + return std::abs(cellPosition.first) + std::abs(cellPosition.second); + } + } @@ -233,15 +332,29 @@ namespace MWWorld void Scene::unloadCell (CellStoreCollection::iterator iter) { Log(Debug::Info) << "Unloading cell " << (*iter)->getCell()->getDescription(); + DetourNavigator::log("unload 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) + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + const auto playerHalfExtents = mPhysics->getHalfExtents(player); + for (const auto& ptr : visitor.mObjects) { - mPhysics->remove(*iter2); + if (const auto object = mPhysics->getObject(ptr)) + navigator->removeObject(DetourNavigator::ObjectId(object)); + else if (const auto actor = mPhysics->getActor(ptr)) + { + navigator->removeAgent(ptr.getCell()->isExterior() ? playerHalfExtents : actor->getHalfExtents()); + mRendering.removeActorPath(ptr); + } + mPhysics->remove(ptr); } + const auto cellX = (*iter)->getCell()->getGridX(); + const auto cellY = (*iter)->getCell()->getGridY(); + if ((*iter)->getCell()->isExterior()) { const ESM::Land* land = @@ -250,9 +363,18 @@ namespace MWWorld (*iter)->getCell()->getGridY() ); if (land && land->mDataTypes&ESM::Land::DATA_VHGT) - mPhysics->removeHeightField ((*iter)->getCell()->getGridX(), (*iter)->getCell()->getGridY()); + { + if (const auto heightField = mPhysics->getHeightField(cellX, cellY)) + navigator->removeObject(DetourNavigator::ObjectId(heightField)); + mPhysics->removeHeightField(cellX, cellY); + } } + if ((*iter)->getCell()->hasWater()) + navigator->removeWater(osg::Vec2i(cellX, cellY)); + + navigator->update(player.getRefData().getPosition().asVec3()); + MWBase::Environment::get().getMechanicsManager()->drop (*iter); mRendering.removeCell(*iter); @@ -271,20 +393,24 @@ namespace MWWorld if(result.second) { Log(Debug::Info) << "Loading cell " << cell->getCell()->getDescription(); + DetourNavigator::log("load cell ", cell->getCell()->getDescription()); float verts = ESM::Land::LAND_SIZE; float worldsize = ESM::Land::REAL_SIZE; + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + + const int cellX = cell->getCell()->getGridX(); + const int cellY = cell->getCell()->getGridY(); + // Load terrain physics first... if (cell->getCell()->isExterior()) { - int cellX = cell->getCell()->getGridX(); - int cellY = cell->getCell()->getGridY(); osg::ref_ptr land = mRendering.getLandManager()->getLand(cellX, cellY); const ESM::Land::LandData* data = land ? land->getData(ESM::Land::DATA_VHGT) : 0; if (data) { - mPhysics->addHeightField (data->mHeights, cellX, cell->getCell()->getGridY(), worldsize / (verts-1), verts, data->mMinHeight, data->mMaxHeight, land.get()); + mPhysics->addHeightField (data->mHeights, cellX, cellY, worldsize / (verts-1), verts, data->mMinHeight, data->mMaxHeight, land.get()); } else { @@ -292,6 +418,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(DetourNavigator::ObjectId(heightField), *heightField->getShape(), + heightField->getCollisionObject()->getWorldTransform()); } // register local scripts @@ -313,10 +443,25 @@ namespace MWWorld { mPhysics->enableWater(waterLevel); mRendering.setWaterHeight(waterLevel); + + if (cell->getCell()->isExterior()) + { + if (const auto heightField = mPhysics->getHeightField(cellX, cellY)) + navigator->addWater(osg::Vec2i(cellX, cellY), ESM::Land::REAL_SIZE, + cell->getWaterLevel(), heightField->getCollisionObject()->getWorldTransform()); + } + else + { + navigator->addWater(osg::Vec2i(cellX, cellY), std::numeric_limits::max(), + cell->getWaterLevel(), btTransform::getIdentity()); + } } else mPhysics->disableWater(); + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + navigator->update(player.getRefData().getPosition().asVec3()); + if (!cell->isExterior() && !(cell->getCell()->mData.mFlags & ESM::Cell::QuasiEx)) mRendering.configureAmbient(cell->getCell()); } @@ -337,6 +482,10 @@ namespace MWWorld void Scene::playerMoved(const osg::Vec3f &pos) { + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + navigator->update(player.getRefData().getPosition().asVec3()); + if (!mCurrentCell || !mCurrentCell->isExterior()) return; @@ -355,7 +504,7 @@ namespace MWWorld } } - void Scene::changeCellGrid (int X, int Y, bool changeEvent) + void Scene::changeCellGrid (int playerCellX, int playerCellY, bool changeEvent) { Loading::Listener* loadingListener = MWBase::Environment::get().getWindowManager()->getLoadingScreen(); Loading::ScopedLoad load(loadingListener); @@ -369,8 +518,8 @@ namespace MWWorld { if ((*active)->getCell()->isExterior()) { - if (std::abs (X-(*active)->getCell()->getGridX())<=mHalfGridSize && - std::abs (Y-(*active)->getCell()->getGridY())<=mHalfGridSize) + if (std::abs (playerCellX-(*active)->getCell()->getGridX())<=mHalfGridSize && + std::abs (playerCellY-(*active)->getCell()->getGridY())<=mHalfGridSize) { // keep cells within the new grid ++active; @@ -380,11 +529,12 @@ namespace MWWorld unloadCell (active++); } - int refsToLoad = 0; + std::size_t refsToLoad = 0; + std::vector> cellsPositionsToLoad; // get the number of refs to load - for (int x=X-mHalfGridSize; x<=X+mHalfGridSize; ++x) + for (int x = playerCellX - mHalfGridSize; x <= playerCellX + mHalfGridSize; ++x) { - for (int y=Y-mHalfGridSize; y<=Y+mHalfGridSize; ++y) + for (int y = playerCellY - mHalfGridSize; y <= playerCellY + mHalfGridSize; ++y) { CellStoreCollection::iterator iter = mActiveCells.begin(); @@ -400,40 +550,58 @@ namespace MWWorld } if (iter==mActiveCells.end()) + { refsToLoad += MWBase::Environment::get().getWorld()->getExterior(x, y)->count(); + cellsPositionsToLoad.push_back(std::make_pair(x, y)); + } } } loadingListener->setProgressRange(refsToLoad); - // Load cells - for (int x=X-mHalfGridSize; x<=X+mHalfGridSize; ++x) + const auto getDistanceToPlayerCell = [&] (const std::pair& cellPosition) { - for (int y=Y-mHalfGridSize; y<=Y+mHalfGridSize; ++y) + return std::abs(cellPosition.first - playerCellX) + std::abs(cellPosition.second - playerCellY); + }; + + const auto getCellPositionPriority = [&] (const std::pair& cellPosition) + { + return std::make_pair(getDistanceToPlayerCell(cellPosition), getCellPositionDistanceToOrigin(cellPosition)); + }; + + std::sort(cellsPositionsToLoad.begin(), cellsPositionsToLoad.end(), + [&] (const std::pair& lhs, const std::pair& rhs) { + return getCellPositionPriority(lhs) < getCellPositionPriority(rhs); + }); + + // Load cells + for (const auto& cellPosition : cellsPositionsToLoad) + { + const auto x = cellPosition.first; + const auto y = cellPosition.second; + + CellStoreCollection::iterator iter = mActiveCells.begin(); + + while (iter != mActiveCells.end()) { - CellStoreCollection::iterator iter = mActiveCells.begin(); + assert ((*iter)->getCell()->isExterior()); - while (iter!=mActiveCells.end()) - { - assert ((*iter)->getCell()->isExterior()); + if (x == (*iter)->getCell()->getGridX() && + y == (*iter)->getCell()->getGridY()) + break; - if (x==(*iter)->getCell()->getGridX() && - y==(*iter)->getCell()->getGridY()) - break; + ++iter; + } - ++iter; - } + if (iter == mActiveCells.end()) + { + CellStore *cell = MWBase::Environment::get().getWorld()->getExterior(x, y); - if (iter==mActiveCells.end()) - { - CellStore *cell = MWBase::Environment::get().getWorld()->getExterior(x, y); - - loadCell (cell, loadingListener, changeEvent); - } + loadCell (cell, loadingListener, changeEvent); } } - CellStore* current = MWBase::Environment::get().getWorld()->getExterior(X,Y); + CellStore* current = MWBase::Environment::get().getWorld()->getExterior(playerCellX, playerCellY); MWBase::Environment::get().getWindowManager()->changeCell(current); if (changeEvent) @@ -477,8 +645,9 @@ namespace MWWorld mLastPlayerPos = pos.asVec3(); } - Scene::Scene (MWRender::RenderingManager& rendering, MWPhysics::PhysicsSystem *physics) - : mCurrentCell (0), mCellChanged (false), mPhysics(physics), mRendering(rendering) + Scene::Scene (MWRender::RenderingManager& rendering, MWPhysics::PhysicsSystem *physics, + DetourNavigator::Navigator& navigator) + : mCurrentCell (0), mCellChanged (false), mPhysics(physics), mRendering(rendering), mNavigator(navigator) , mPreloadTimer(0.f) , mHalfGridSize(Settings::Manager::getInt("exterior cell load distance", "Cells")) , mCellLoadingThreshold(1024.f) @@ -555,8 +724,7 @@ namespace MWWorld while (active!=mActiveCells.end()) unloadCell (active++); - int refsToLoad = cell->count(); - loadingListener->setProgressRange(refsToLoad); + loadingListener->setProgressRange(cell->count()); // Load cell. loadCell (cell, loadingListener, changeEvent); @@ -608,9 +776,10 @@ namespace MWWorld void Scene::insertCell (CellStore &cell, bool rescale, Loading::Listener* loadingListener) { - InsertVisitor insertVisitor (cell, rescale, *loadingListener, *mPhysics, mRendering); + InsertVisitor insertVisitor (cell, rescale, *loadingListener); cell.forEach (insertVisitor); - insertVisitor.insert(); + insertVisitor.insert([&] (const MWWorld::Ptr& ptr) { addObject(ptr, *mPhysics, mRendering); }); + insertVisitor.insert([&] (const MWWorld::Ptr& ptr) { addObject(ptr, *mPhysics, mNavigator); }); // do adjustPosition (snapping actors to ground) after objects are loaded, so we don't depend on the loading order AdjustPositionVisitor adjustPosVisitor; @@ -622,7 +791,11 @@ namespace MWWorld try { addObject(ptr, *mPhysics, mRendering); + addObject(ptr, *mPhysics, mNavigator); MWBase::Environment::get().getWorld()->scaleObject(ptr, ptr.getCellRef().getScale()); + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + navigator->update(player.getRefData().getPosition().asVec3()); } catch (std::exception& e) { @@ -634,6 +807,20 @@ 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(DetourNavigator::ObjectId(object)); + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + navigator->update(player.getRefData().getPosition().asVec3()); + } + else if (const auto actor = mPhysics->getActor(ptr)) + { + const auto& halfExtents = ptr.getCell()->isExterior() + ? mPhysics->getHalfExtents(MWBase::Environment::get().getWorld()->getPlayerPtr()) + : actor->getHalfExtents(); + navigator->removeAgent(halfExtents); + } mPhysics->remove(ptr); mRendering.removeObject (ptr); if (ptr.getClass().isActor()) diff --git a/apps/openmw/mwworld/scene.hpp b/apps/openmw/mwworld/scene.hpp index e2fac64383..00f5f98b83 100644 --- a/apps/openmw/mwworld/scene.hpp +++ b/apps/openmw/mwworld/scene.hpp @@ -6,6 +6,7 @@ #include #include +#include namespace osg { @@ -27,6 +28,12 @@ namespace Loading class Listener; } +namespace DetourNavigator +{ + class Navigator; + class Water; +} + namespace MWRender { class SkyManager; @@ -57,6 +64,7 @@ namespace MWWorld bool mCellChanged; MWPhysics::PhysicsSystem *mPhysics; MWRender::RenderingManager& mRendering; + DetourNavigator::Navigator& mNavigator; std::unique_ptr mPreloader; float mPreloadTimer; int mHalfGridSize; @@ -74,7 +82,7 @@ namespace MWWorld void insertCell (CellStore &cell, bool rescale, Loading::Listener* loadingListener); // Load and unload cells as necessary to create a cell grid with "X" and "Y" in the center - void changeCellGrid (int X, int Y, bool changeEvent = true); + void changeCellGrid (int playerCellX, int playerCellY, bool changeEvent = true); void getGridCenter(int& cellX, int& cellY); @@ -85,7 +93,8 @@ namespace MWWorld public: - Scene (MWRender::RenderingManager& rendering, MWPhysics::PhysicsSystem *physics); + Scene (MWRender::RenderingManager& rendering, MWPhysics::PhysicsSystem *physics, + DetourNavigator::Navigator& navigator); ~Scene(); diff --git a/apps/openmw/mwworld/worldimp.cpp b/apps/openmw/mwworld/worldimp.cpp index 1e267372ac..30c1292a78 100644 --- a/apps/openmw/mwworld/worldimp.cpp +++ b/apps/openmw/mwworld/worldimp.cpp @@ -3,6 +3,9 @@ #include #include +#include +#include + #include #include @@ -15,10 +18,15 @@ #include +#include #include #include +#include +#include +#include + #include "../mwbase/environment.hpp" #include "../mwbase/soundmanager.hpp" #include "../mwbase/mechanicsmanager.hpp" @@ -47,6 +55,7 @@ #include "../mwphysics/physicssystem.hpp" #include "../mwphysics/actor.hpp" #include "../mwphysics/collisiontype.hpp" +#include "../mwphysics/object.hpp" #include "player.hpp" #include "manualref.hpp" @@ -159,12 +168,6 @@ namespace MWWorld mLevitationEnabled(true), mGoToJail(false), mDaysInPrison(0), mPlayerTraveling(false), mPlayerInJail(false), mSpellPreloadTimer(0.f) { - mPhysics.reset(new MWPhysics::PhysicsSystem(resourceSystem, rootNode)); - mRendering.reset(new MWRender::RenderingManager(viewer, rootNode, resourceSystem, workQueue, &mFallback, resourcePath)); - mProjectileManager.reset(new ProjectileManager(mRendering->getLightRoot(), resourceSystem, mRendering.get(), mPhysics.get())); - - mRendering->preloadCommonAssets(); - mEsm.resize(contentFiles.size()); Loading::Listener* listener = MWBase::Environment::get().getWindowManager()->getLoadingScreen(); listener->loadingOn(); @@ -193,9 +196,49 @@ namespace MWWorld mSwimHeightScale = mStore.get().find("fSwimHeightScale")->mValue.getFloat(); + mPhysics.reset(new MWPhysics::PhysicsSystem(resourceSystem, rootNode)); + + DetourNavigator::Settings navigatorSettings; + navigatorSettings.mBorderSize = Settings::Manager::getInt("border size", "Navigator"); + 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.mSwimHeightScale = mSwimHeightScale; + navigatorSettings.mMaxEdgeLen = Settings::Manager::getInt("max edge len", "Navigator"); + navigatorSettings.mMaxNavMeshQueryNodes = Settings::Manager::getInt("max nav mesh query nodes", "Navigator"); + navigatorSettings.mMaxPolys = Settings::Manager::getInt("max polygons per tile", "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.mAsyncNavMeshUpdaterThreads = static_cast(Settings::Manager::getInt("async nav mesh updater threads", "Navigator")); + navigatorSettings.mMaxNavMeshTilesCacheSize = static_cast(Settings::Manager::getInt("max nav mesh tiles cache 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")); + navigatorSettings.mEnableWriteRecastMeshToFile = Settings::Manager::getBool("enable write recast mesh to file", "Navigator"); + navigatorSettings.mEnableWriteNavMeshToFile = Settings::Manager::getBool("enable write nav mesh to file", "Navigator"); + navigatorSettings.mRecastMeshPathPrefix = Settings::Manager::getString("recast mesh path prefix", "Navigator"); + navigatorSettings.mNavMeshPathPrefix = Settings::Manager::getString("nav mesh path prefix", "Navigator"); + navigatorSettings.mEnableRecastMeshFileNameRevision = Settings::Manager::getBool("enable recast mesh file name revision", "Navigator"); + navigatorSettings.mEnableNavMeshFileNameRevision = Settings::Manager::getBool("enable nav mesh file name revision", "Navigator"); + if (Settings::Manager::getBool("enable log", "Navigator")) + DetourNavigator::Log::instance().setSink(std::unique_ptr( + new DetourNavigator::FileSink(Settings::Manager::getString("log path", "Navigator")))); + mNavigator.reset(new DetourNavigator::Navigator(navigatorSettings)); + + mRendering.reset(new MWRender::RenderingManager(viewer, rootNode, resourceSystem, workQueue, &mFallback, resourcePath, *mNavigator)); + mProjectileManager.reset(new ProjectileManager(mRendering->getLightRoot(), resourceSystem, mRendering.get(), mPhysics.get())); + mRendering->preloadCommonAssets(); + mWeatherManager.reset(new MWWorld::WeatherManager(*mRendering, mFallback, mStore)); - mWorldScene.reset(new Scene(*mRendering.get(), mPhysics.get())); + mWorldScene.reset(new Scene(*mRendering.get(), mPhysics.get(), *mNavigator)); } void World::fillGlobalVariables() @@ -1506,6 +1549,32 @@ namespace MWWorld moveObjectImp(player->first, player->second.x(), player->second.y(), player->second.z(), false); } + void World::updateNavigator() + { + bool updated = false; + + mPhysics->forEachAnimatedObject([&] (const MWPhysics::Object* object) + { + updated = updateNavigatorObject(object) || updated; + }); + + for (const auto& door : mDoorStates) + if (const auto object = mPhysics->getObject(door.first)) + updated = updateNavigatorObject(object) || updated; + + if (updated) + mNavigator->update(getPlayerPtr().getRefData().getPosition().asVec3()); + } + + bool World::updateNavigatorObject(const MWPhysics::Object* object) + { + const DetourNavigator::ObjectShapes shapes { + *object->getShapeInstance()->getCollisionShape(), + object->getShapeInstance()->getAvoidCollisionShape() + }; + return mNavigator->updateObject(DetourNavigator::ObjectId(object), shapes, object->getCollisionObject()->getWorldTransform()); + } + bool World::castRay (float x1, float y1, float z1, float x2, float y2, float z2) { int mask = MWPhysics::CollisionType_World | MWPhysics::CollisionType_Door; @@ -1708,7 +1777,10 @@ namespace MWWorld updateWeather(duration, paused); if (!paused) + { doPhysics (duration); + updateNavigator(); + } updatePlayer(); @@ -2236,7 +2308,7 @@ namespace MWWorld float waterlevel = cell->getWaterLevel(); - // SwimHeightScale affects the upper z position an actor can swim to + // SwimHeightScale affects the upper z position an actor can swim to // while in water. Based on observation from the original engine, // the upper z position you get with a +1 SwimHeightScale is the depth // limit for being able to cast water walking on an underwater target. @@ -2305,6 +2377,7 @@ namespace MWWorld { // Remove the old CharacterController MWBase::Environment::get().getMechanicsManager()->remove(getPlayerPtr()); + mNavigator->removeAgent(mPhysics->getHalfExtents(getPlayerPtr())); mPhysics->remove(getPlayerPtr()); mRendering->removePlayer(getPlayerPtr()); @@ -2339,6 +2412,8 @@ namespace MWWorld mPhysics->addActor(getPlayerPtr(), model); applyLoopingParticles(player); + + mNavigator->addAgent(mPhysics->getHalfExtents(getPlayerPtr())); } World::RestPermitted World::canRest () const @@ -2379,7 +2454,7 @@ namespace MWWorld { mRendering->screenshot(image, w, h); } - + bool World::screenshot360(osg::Image* image, std::string settingStr) { return mRendering->screenshot360(image,settingStr); @@ -3552,7 +3627,7 @@ namespace MWWorld continue; } else - mRendering->spawnEffect("meshes\\" + areaStatic->mModel, texture, origin, static_cast(effectIt->mArea * 2)); + mRendering->spawnEffect("meshes\\" + areaStatic->mModel, texture, origin, static_cast(effectIt->mArea * 2)); // Play explosion sound (make sure to use NoTrack, since we will delete the projectile now) static const std::string schools[] = { @@ -3704,4 +3779,20 @@ namespace MWWorld } } + DetourNavigator::Navigator* World::getNavigator() const + { + return mNavigator.get(); + } + + void World::updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const + { + mRendering->updateActorPath(actor, path, halfExtents, start, end); + } + + void World::setNavMeshNumberToRender(const std::size_t value) + { + mRendering->setNavMeshNumber(value); + } + } diff --git a/apps/openmw/mwworld/worldimp.hpp b/apps/openmw/mwworld/worldimp.hpp index 10ccb78fea..9e288bd6bf 100644 --- a/apps/openmw/mwworld/worldimp.hpp +++ b/apps/openmw/mwworld/worldimp.hpp @@ -61,6 +61,11 @@ namespace ToUTF8 struct ContentLoader; +namespace MWPhysics +{ + class Object; +} + namespace MWWorld { class WeatherManager; @@ -94,6 +99,7 @@ namespace MWWorld std::unique_ptr mPlayer; std::unique_ptr mPhysics; + std::unique_ptr mNavigator; std::unique_ptr mRendering; std::unique_ptr mWorldScene; std::unique_ptr mWeatherManager; @@ -147,6 +153,10 @@ namespace MWWorld void doPhysics(float duration); ///< Run physics simulation and modify \a world accordingly. + void updateNavigator(); + + bool updateNavigatorObject(const MWPhysics::Object* object); + void ensureNeededRecords(); void fillGlobalVariables(); @@ -693,6 +703,13 @@ namespace MWWorld /// Preload VFX associated with this effect list void preloadEffects(const ESM::EffectList* effectList) override; + + DetourNavigator::Navigator* getNavigator() const override; + + void updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const override; + + void setNavMeshNumberToRender(const std::size_t value) override; }; } diff --git a/apps/openmw_test_suite/CMakeLists.txt b/apps/openmw_test_suite/CMakeLists.txt index bc39bc2be6..acb33e1fa3 100644 --- a/apps/openmw_test_suite/CMakeLists.txt +++ b/apps/openmw_test_suite/CMakeLists.txt @@ -17,10 +17,21 @@ if (GTEST_FOUND AND GMOCK_FOUND) misc/test_stringops.cpp nifloader/testbulletnifloader.cpp + + detournavigator/navigator.cpp + detournavigator/settingsutils.cpp + detournavigator/recastmeshbuilder.cpp + detournavigator/gettilespositions.cpp + detournavigator/recastmeshobject.cpp + detournavigator/navmeshtilescache.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/gettilespositions.cpp b/apps/openmw_test_suite/detournavigator/gettilespositions.cpp new file mode 100644 index 0000000000..1ad5c063d0 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/gettilespositions.cpp @@ -0,0 +1,104 @@ +#include +#include + +#include +#include + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct CollectTilesPositions + { + std::vector& mTilesPositions; + + void operator ()(const TilePosition& value) + { + mTilesPositions.push_back(value); + } + }; + + struct DetourNavigatorGetTilesPositionsTest : Test + { + Settings mSettings; + std::vector mTilesPositions; + CollectTilesPositions mCollect {mTilesPositions}; + + DetourNavigatorGetTilesPositionsTest() + { + mSettings.mBorderSize = 0; + mSettings.mCellSize = 0.5; + mSettings.mRecastScaleFactor = 1; + mSettings.mTileSize = 64; + } + }; + + TEST_F(DetourNavigatorGetTilesPositionsTest, for_object_in_single_tile_should_return_one_tile) + { + getTilesPositions(osg::Vec3f(2, 2, 0), osg::Vec3f(31, 31, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0))); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, for_object_with_x_bounds_in_two_tiles_should_return_two_tiles) + { + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(32, 31, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0), TilePosition(1, 0))); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, for_object_with_y_bounds_in_two_tiles_should_return_two_tiles) + { + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(31, 32, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0), TilePosition(0, 1))); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, tiling_works_only_for_x_and_y_coordinates) + { + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(31, 31, 32), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0))); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, tiling_should_work_with_negative_coordinates) + { + getTilesPositions(osg::Vec3f(-31, -31, 0), osg::Vec3f(31, 31, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre( + TilePosition(-1, -1), + TilePosition(-1, 0), + TilePosition(0, -1), + TilePosition(0, 0) + )); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, border_size_should_extend_tile_bounds) + { + mSettings.mBorderSize = 1; + + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(31.5, 31.5, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre( + TilePosition(-1, -1), + TilePosition(-1, 0), + TilePosition(-1, 1), + TilePosition(0, -1), + TilePosition(0, 0), + TilePosition(0, 1), + TilePosition(1, -1), + TilePosition(1, 0), + TilePosition(1, 1) + )); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, should_apply_recast_scale_factor) + { + mSettings.mRecastScaleFactor = 0.5; + + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(32, 32, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0))); + } +} diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp new file mode 100644 index 0000000000..febbc03875 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -0,0 +1,661 @@ +#include "operators.hpp" + +#include +#include + +#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.mEnableWriteRecastMeshToFile = false; + mSettings.mEnableWriteNavMeshToFile = false; + mSettings.mEnableRecastMeshFileNameRevision = false; + mSettings.mEnableNavMeshFileNameRevision = false; + mSettings.mBorderSize = 16; + 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.mSwimHeightScale = 0.89999997615814208984375f; + mSettings.mMaxEdgeLen = 12; + mSettings.mMaxNavMeshQueryNodes = 2048; + mSettings.mMaxVertsPerPoly = 6; + mSettings.mRegionMergeSize = 20; + mSettings.mRegionMinSize = 8; + mSettings.mTileSize = 64; + mSettings.mAsyncNavMeshUpdaterThreads = 1; + mSettings.mMaxNavMeshTilesCacheSize = 1024 * 1024; + mSettings.mMaxPolygonPathSize = 1024; + mSettings.mMaxSmoothPathSize = 1024; + mSettings.mTrianglesPerChunk = 256; + mSettings.mMaxPolys = 4096; + mNavigator.reset(new Navigator(mSettings)); + } + }; + + TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_throw_exception) + { + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), InvalidArgument); + } + + TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception) + { + mNavigator->addAgent(mAgentHalfExtents); + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), NavigatorException); + } + + TEST_F(DetourNavigatorNavigatorTest, find_path_for_removed_agent_should_throw_exception) + { + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->removeAgent(mAgentHalfExtents); + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), InvalidArgument); + } + + TEST_F(DetourNavigatorNavigatorTest, add_agent_should_count_each_agent) + { + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->removeAgent(mAgentHalfExtents); + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), NavigatorException); + } + + 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(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, 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, add_object_should_change_navmesh) + { + const std::array heightfieldData {{ + 0, 0, 0, 0, 0, + 0, -25, -25, -25, -25, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + }}; + btHeightfieldTerrainShape heightfieldShape(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + heightfieldShape.setLocalScaling(btVector3(128, 128, 1)); + + btBoxShape boxShape(btVector3(20, 20, 100)); + btCompoundShape compoundShape; + compoundShape.addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(0, 0, 0)), &boxShape); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(ObjectId(&heightfieldShape), heightfieldShape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); + + 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; + + mNavigator->addObject(ObjectId(&compoundShape), compoundShape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mPath.clear(); + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(-215, 215, 1.87827122211456298828125), + osg::Vec3f(-199.7968292236328125, 191.09100341796875, -3.54876613616943359375), + osg::Vec3f(-184.5936431884765625, 167.1819915771484375, -8.97847270965576171875), + osg::Vec3f(-169.3904571533203125, 143.2729949951171875, -14.40817737579345703125), + osg::Vec3f(-154.1872711181640625, 119.36397552490234375, -19.837890625), + osg::Vec3f(-138.9840850830078125, 95.45496368408203125, -25.2675952911376953125), + osg::Vec3f(-123.78090667724609375, 71.54595184326171875, -30.6972980499267578125), + osg::Vec3f(-108.57772064208984375, 47.636936187744140625, -36.12701416015625), + osg::Vec3f(-93.3745269775390625, 23.7279262542724609375, -40.754688262939453125), + osg::Vec3f(-78.17134857177734375, -0.18108306825160980224609375, -37.128787994384765625), + osg::Vec3f(-62.968158721923828125, -24.0900936126708984375, -33.50289154052734375), + osg::Vec3f(-47.764972686767578125, -47.999103546142578125, -30.797946929931640625), + osg::Vec3f(-23.852447509765625, -63.196765899658203125, -33.97112274169921875), + osg::Vec3f(0.0600789971649646759033203125, -78.39443206787109375, -37.14543914794921875), + osg::Vec3f(23.97260284423828125, -93.5920867919921875, -40.7740936279296875), + osg::Vec3f(47.885128021240234375, -108.78974151611328125, -36.051288604736328125), + osg::Vec3f(71.7976531982421875, -123.98740386962890625, -30.62355804443359375), + osg::Vec3f(95.71018218994140625, -139.18505859375, -25.1958160400390625), + osg::Vec3f(119.6226959228515625, -154.382720947265625, -19.7680912017822265625), + osg::Vec3f(143.53521728515625, -169.58038330078125, -14.3403491973876953125), + osg::Vec3f(167.4477386474609375, -184.778045654296875, -8.91261768341064453125), + osg::Vec3f(191.360260009765625, -199.9757080078125, -3.484879016876220703125), + osg::Vec3f(215, -215, 1.87827455997467041015625), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, update_changed_object_should_change_navmesh) + { + const std::array heightfieldData {{ + 0, 0, 0, 0, 0, + 0, -25, -25, -25, -25, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + }}; + btHeightfieldTerrainShape heightfieldShape(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + heightfieldShape.setLocalScaling(btVector3(128, 128, 1)); + + btBoxShape boxShape(btVector3(20, 20, 100)); + btCompoundShape compoundShape; + compoundShape.addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(0, 0, 0)), &boxShape); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(ObjectId(&heightfieldShape), heightfieldShape, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&compoundShape), compoundShape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(-215, 215, 1.87827122211456298828125), + osg::Vec3f(-199.7968292236328125, 191.09100341796875, -3.54876613616943359375), + osg::Vec3f(-184.5936431884765625, 167.1819915771484375, -8.97847270965576171875), + osg::Vec3f(-169.3904571533203125, 143.2729949951171875, -14.40817737579345703125), + osg::Vec3f(-154.1872711181640625, 119.36397552490234375, -19.837890625), + osg::Vec3f(-138.9840850830078125, 95.45496368408203125, -25.2675952911376953125), + osg::Vec3f(-123.78090667724609375, 71.54595184326171875, -30.6972980499267578125), + osg::Vec3f(-108.57772064208984375, 47.636936187744140625, -36.12701416015625), + osg::Vec3f(-93.3745269775390625, 23.7279262542724609375, -40.754688262939453125), + osg::Vec3f(-78.17134857177734375, -0.18108306825160980224609375, -37.128787994384765625), + osg::Vec3f(-62.968158721923828125, -24.0900936126708984375, -33.50289154052734375), + osg::Vec3f(-47.764972686767578125, -47.999103546142578125, -30.797946929931640625), + osg::Vec3f(-23.852447509765625, -63.196765899658203125, -33.97112274169921875), + osg::Vec3f(0.0600789971649646759033203125, -78.39443206787109375, -37.14543914794921875), + osg::Vec3f(23.97260284423828125, -93.5920867919921875, -40.7740936279296875), + osg::Vec3f(47.885128021240234375, -108.78974151611328125, -36.051288604736328125), + osg::Vec3f(71.7976531982421875, -123.98740386962890625, -30.62355804443359375), + osg::Vec3f(95.71018218994140625, -139.18505859375, -25.1958160400390625), + osg::Vec3f(119.6226959228515625, -154.382720947265625, -19.7680912017822265625), + osg::Vec3f(143.53521728515625, -169.58038330078125, -14.3403491973876953125), + osg::Vec3f(167.4477386474609375, -184.778045654296875, -8.91261768341064453125), + osg::Vec3f(191.360260009765625, -199.9757080078125, -3.484879016876220703125), + osg::Vec3f(215, -215, 1.87827455997467041015625), + })) << mPath; + + compoundShape.updateChildTransform(0, btTransform(btMatrix3x3::getIdentity(), btVector3(1000, 0, 0))); + + mNavigator->updateObject(ObjectId(&compoundShape), compoundShape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mPath.clear(); + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, 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(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&shape2), shape2, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, 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; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_around_avoid_shape) + { + 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)); + + std::array heightfieldDataAvoid {{ + -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 shapeAvoid(5, 5, heightfieldDataAvoid.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shapeAvoid.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(ObjectId(&shape), ObjectShapes {shape, &shapeAvoid}, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(-215, 215, 1.9393787384033203125), + osg::Vec3f(-200.8159637451171875, 190.47265625, -0.639537751674652099609375), + osg::Vec3f(-186.6319427490234375, 165.9453125, -3.2184507846832275390625), + osg::Vec3f(-172.447906494140625, 141.41796875, -5.797363758087158203125), + osg::Vec3f(-158.263885498046875, 116.8906097412109375, -8.37627887725830078125), + osg::Vec3f(-144.079864501953125, 92.3632659912109375, -10.95519161224365234375), + osg::Vec3f(-129.89581298828125, 67.83591461181640625, -13.534107208251953125), + osg::Vec3f(-115.7117919921875, 43.308563232421875, -16.1130199432373046875), + osg::Vec3f(-101.5277557373046875, 18.7812137603759765625, -18.6919345855712890625), + osg::Vec3f(-87.34372711181640625, -5.7461376190185546875, -20.4680538177490234375), + osg::Vec3f(-67.02922821044921875, -25.4970550537109375, -20.514247894287109375), + osg::Vec3f(-46.714717864990234375, -45.2479705810546875, -20.5604457855224609375), + osg::Vec3f(-26.40021514892578125, -64.99889373779296875, -20.6066417694091796875), + osg::Vec3f(-6.085712432861328125, -84.74980926513671875, -20.652835845947265625), + osg::Vec3f(14.22879505157470703125, -104.50072479248046875, -18.151393890380859375), + osg::Vec3f(39.05098724365234375, -118.16222381591796875, -15.6674861907958984375), + osg::Vec3f(63.87317657470703125, -131.82373046875, -13.18357944488525390625), + osg::Vec3f(88.69537353515625, -145.4852142333984375, -10.69967365264892578125), + osg::Vec3f(113.51757049560546875, -159.146697998046875, -8.21576690673828125), + osg::Vec3f(138.3397674560546875, -172.808197021484375, -5.731858730316162109375), + osg::Vec3f(163.1619720458984375, -186.469696044921875, -3.2479503154754638671875), + osg::Vec3f(187.984161376953125, -200.1311798095703125, -0.764044582843780517578125), + osg::Vec3f(212.8063507080078125, -213.7926788330078125, 1.7198636531829833984375), + osg::Vec3f(215, -215, 1.93937528133392333984375), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_ground_lower_than_water_with_only_swim_flag) + { + std::array heightfieldData {{ + -50, -50, -50, -50, 0, + -50, -100, -150, -100, -50, + -50, -150, -200, -150, -100, + -50, -100, -150, -100, -100, + 0, -50, -100, -100, -100, + }}; + btHeightfieldTerrainShape shape(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, 300, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mStart.x() = 0; + mStart.z() = 300; + mEnd.x() = 0; + mEnd.z() = 300; + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_swim, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(0, 215, 185.33331298828125), + osg::Vec3f(0, 186.6666717529296875, 185.33331298828125), + osg::Vec3f(0, 158.333343505859375, 185.33331298828125), + osg::Vec3f(0, 130.0000152587890625, 185.33331298828125), + osg::Vec3f(0, 101.66667938232421875, 185.33331298828125), + osg::Vec3f(0, 73.333343505859375, 185.33331298828125), + osg::Vec3f(0, 45.0000152587890625, 185.33331298828125), + osg::Vec3f(0, 16.6666812896728515625, 185.33331298828125), + osg::Vec3f(0, -11.66664981842041015625, 185.33331298828125), + osg::Vec3f(0, -39.999980926513671875, 185.33331298828125), + osg::Vec3f(0, -68.33331298828125, 185.33331298828125), + osg::Vec3f(0, -96.66664886474609375, 185.33331298828125), + osg::Vec3f(0, -124.99997711181640625, 185.33331298828125), + osg::Vec3f(0, -153.33331298828125, 185.33331298828125), + osg::Vec3f(0, -181.6666412353515625, 185.33331298828125), + osg::Vec3f(0, -209.999969482421875, 185.33331298828125), + osg::Vec3f(0, -215, 185.33331298828125), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_when_ground_cross_water_with_swim_and_walk_flags) + { + std::array heightfieldData {{ + 0, 0, 0, 0, 0, 0, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -150, -200, -150, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, 0, 0, 0, 0, 0, 0, + }}; + btHeightfieldTerrainShape shape(7, 7, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, -25, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mStart.x() = 0; + mEnd.x() = 0; + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_swim | Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(0, 215, -94.75363922119140625), + osg::Vec3f(0, 186.6666717529296875, -106.0000152587890625), + osg::Vec3f(0, 158.333343505859375, -115.85507965087890625), + osg::Vec3f(0, 130.0000152587890625, -125.71016693115234375), + osg::Vec3f(0, 101.66667938232421875, -135.5652313232421875), + osg::Vec3f(0, 73.333343505859375, -143.3333587646484375), + osg::Vec3f(0, 45.0000152587890625, -143.3333587646484375), + osg::Vec3f(0, 16.6666812896728515625, -143.3333587646484375), + osg::Vec3f(0, -11.66664981842041015625, -143.3333587646484375), + osg::Vec3f(0, -39.999980926513671875, -143.3333587646484375), + osg::Vec3f(0, -68.33331298828125, -143.3333587646484375), + osg::Vec3f(0, -96.66664886474609375, -137.3043670654296875), + osg::Vec3f(0, -124.99997711181640625, -127.44930267333984375), + osg::Vec3f(0, -153.33331298828125, -117.5942230224609375), + osg::Vec3f(0, -181.6666412353515625, -107.7391510009765625), + osg::Vec3f(0, -209.999969482421875, -97.79712677001953125), + osg::Vec3f(0, -215, -94.753631591796875), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_when_ground_cross_water_with_max_int_cells_size_and_swim_and_walk_flags) + { + std::array heightfieldData {{ + 0, 0, 0, 0, 0, 0, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -150, -200, -150, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, 0, 0, 0, 0, 0, 0, + }}; + btHeightfieldTerrainShape shape(7, 7, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->addWater(osg::Vec2i(0, 0), std::numeric_limits::max(), -25, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mStart.x() = 0; + mEnd.x() = 0; + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_swim | Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(0, 215, -94.75363922119140625), + osg::Vec3f(0, 186.6666717529296875, -106.0000152587890625), + osg::Vec3f(0, 158.333343505859375, -115.85507965087890625), + osg::Vec3f(0, 130.0000152587890625, -125.71016693115234375), + osg::Vec3f(0, 101.66667938232421875, -135.5652313232421875), + osg::Vec3f(0, 73.333343505859375, -143.3333587646484375), + osg::Vec3f(0, 45.0000152587890625, -143.3333587646484375), + osg::Vec3f(0, 16.6666812896728515625, -143.3333587646484375), + osg::Vec3f(0, -11.66664981842041015625, -143.3333587646484375), + osg::Vec3f(0, -39.999980926513671875, -143.3333587646484375), + osg::Vec3f(0, -68.33331298828125, -143.3333587646484375), + osg::Vec3f(0, -96.66664886474609375, -137.3043670654296875), + osg::Vec3f(0, -124.99997711181640625, -127.44930267333984375), + osg::Vec3f(0, -153.33331298828125, -117.5942230224609375), + osg::Vec3f(0, -181.6666412353515625, -107.7391510009765625), + osg::Vec3f(0, -209.999969482421875, -97.79712677001953125), + osg::Vec3f(0, -215, -94.753631591796875), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_ground_when_ground_cross_water_with_only_walk_flag) + { + std::array heightfieldData {{ + 0, 0, 0, 0, 0, 0, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -150, -200, -150, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, 0, 0, 0, 0, 0, 0, + }}; + btHeightfieldTerrainShape shape(7, 7, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, -25, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mStart.x() = 0; + mEnd.x() = 0; + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(0, 215, -94.75363922119140625), + osg::Vec3f(9.8083515167236328125, 188.4185333251953125, -105.19994354248046875), + osg::Vec3f(19.6167049407958984375, 161.837066650390625, -114.25496673583984375), + osg::Vec3f(29.42505645751953125, 135.255615234375, -123.309967041015625), + osg::Vec3f(39.23340606689453125, 108.674163818359375, -132.3649749755859375), + osg::Vec3f(49.04175567626953125, 82.09270477294921875, -137.2874755859375), + osg::Vec3f(58.8501129150390625, 55.5112457275390625, -139.2451171875), + osg::Vec3f(68.6584625244140625, 28.9297885894775390625, -141.2027740478515625), + osg::Vec3f(78.4668121337890625, 2.3483295440673828125, -143.1604156494140625), + osg::Vec3f(88.27516937255859375, -24.233127593994140625, -141.3894805908203125), + osg::Vec3f(83.73651885986328125, -52.2005767822265625, -142.3761444091796875), + osg::Vec3f(79.19786834716796875, -80.16802978515625, -143.114837646484375), + osg::Vec3f(64.8477935791015625, -104.598602294921875, -137.840911865234375), + osg::Vec3f(50.497714996337890625, -129.0291748046875, -131.45831298828125), + osg::Vec3f(36.147632598876953125, -153.459747314453125, -121.42321014404296875), + osg::Vec3f(21.7975559234619140625, -177.8903350830078125, -111.38809967041015625), + osg::Vec3f(7.44747829437255859375, -202.3209075927734375, -101.1938323974609375), + osg::Vec3f(0, -215, -94.753631591796875), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, update_remove_and_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(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->removeObject(ObjectId(&shape)); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, 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; + } +} diff --git a/apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp b/apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp new file mode 100644 index 0000000000..93fc5fbaf6 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp @@ -0,0 +1,312 @@ +#include "operators.hpp" + +#include +#include +#include + +#include + +#include + +namespace DetourNavigator +{ + static inline bool operator ==(const NavMeshDataRef& lhs, const NavMeshDataRef& rhs) + { + return std::make_pair(lhs.mValue, lhs.mSize) == std::make_pair(rhs.mValue, rhs.mSize); + } +} + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorNavMeshTilesCacheTest : Test + { + const osg::Vec3f mAgentHalfExtents {1, 2, 3}; + const TilePosition mTilePosition {0, 0}; + const std::vector mIndices {{0, 1, 2}}; + const std::vector mVertices {{0, 0, 0, 1, 0, 0, 1, 1, 0}}; + const std::vector mAreaTypes {1, AreaType_ground}; + const std::vector mWater {}; + const std::size_t mTrianglesPerChunk {1}; + const RecastMesh mRecastMesh {mIndices, mVertices, mAreaTypes, mWater, mTrianglesPerChunk}; + const std::vector mOffMeshConnections {}; + unsigned char* const mData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData mNavMeshData {mData, 1}; + }; + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_for_empty_cache_should_return_empty_value) + { + const std::size_t maxSize = 0; + NavMeshTilesCache cache(maxSize); + + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_for_not_enought_cache_size_should_return_empty_value) + { + const std::size_t maxSize = 0; + NavMeshTilesCache cache(maxSize); + + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData))); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_return_cached_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + + const auto result = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + EXPECT_EQ(result.get(), (NavMeshDataRef {mData, 1})); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_existing_element_should_throw_exception) + { + const std::size_t maxSize = 2; + NavMeshTilesCache cache(maxSize); + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_THROW( + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(anotherNavMeshData)), + InvalidArgument + ); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_should_return_cached_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + const auto result = cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections); + ASSERT_TRUE(result); + EXPECT_EQ(result.get(), (NavMeshDataRef {mData, 1})); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_for_cache_miss_by_agent_half_extents_should_return_empty_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + const osg::Vec3f unexsistentAgentHalfExtents {1, 1, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_FALSE(cache.get(unexsistentAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_for_cache_miss_by_tile_position_should_return_empty_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + const TilePosition unexistentTilePosition {1, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_FALSE(cache.get(mAgentHalfExtents, unexistentTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_for_cache_miss_by_recast_mesh_should_return_empty_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh unexistentRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, unexistentRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_replace_unused_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + const auto result = cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData)); + ASSERT_TRUE(result); + EXPECT_EQ(result.get(), (NavMeshDataRef {anotherData, 1})); + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_not_replace_used_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + const auto value = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData))); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_replace_unused_least_recently_set_value) + { + const std::size_t maxSize = 2; + NavMeshTilesCache cache(maxSize); + + const std::vector leastRecentlySetWater {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh leastRecentlySetRecastMesh {mIndices, mVertices, mAreaTypes, leastRecentlySetWater, + mTrianglesPerChunk}; + const auto leastRecentlySetData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData leastRecentlySetNavMeshData {leastRecentlySetData, 1}; + + const std::vector mostRecentlySetWater {1, RecastMesh::Water {2, btTransform::getIdentity()}}; + const RecastMesh mostRecentlySetRecastMesh {mIndices, mVertices, mAreaTypes, mostRecentlySetWater, + mTrianglesPerChunk}; + const auto mostRecentlySetData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData mostRecentlySetNavMeshData {mostRecentlySetData, 1}; + + ASSERT_TRUE(cache.set(mAgentHalfExtents, mTilePosition, leastRecentlySetRecastMesh, mOffMeshConnections, + std::move(leastRecentlySetNavMeshData))); + ASSERT_TRUE(cache.set(mAgentHalfExtents, mTilePosition, mostRecentlySetRecastMesh, mOffMeshConnections, + std::move(mostRecentlySetNavMeshData))); + + const auto result = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + EXPECT_EQ(result.get(), (NavMeshDataRef {mData, 1})); + + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, leastRecentlySetRecastMesh, mOffMeshConnections)); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mostRecentlySetRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_replace_unused_least_recently_used_value) + { + const std::size_t maxSize = 2; + NavMeshTilesCache cache(maxSize); + + const std::vector leastRecentlyUsedWater {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh leastRecentlyUsedRecastMesh {mIndices, mVertices, mAreaTypes, leastRecentlyUsedWater, + mTrianglesPerChunk}; + const auto leastRecentlyUsedData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData leastRecentlyUsedNavMeshData {leastRecentlyUsedData, 1}; + + const std::vector mostRecentlyUsedWater {1, RecastMesh::Water {2, btTransform::getIdentity()}}; + const RecastMesh mostRecentlyUsedRecastMesh {mIndices, mVertices, mAreaTypes, mostRecentlyUsedWater, + mTrianglesPerChunk}; + const auto mostRecentlyUsedData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData mostRecentlyUsedNavMeshData {mostRecentlyUsedData, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, leastRecentlyUsedRecastMesh, mOffMeshConnections, + std::move(leastRecentlyUsedNavMeshData)); + cache.set(mAgentHalfExtents, mTilePosition, mostRecentlyUsedRecastMesh, mOffMeshConnections, + std::move(mostRecentlyUsedNavMeshData)); + + { + const auto value = cache.get(mAgentHalfExtents, mTilePosition, leastRecentlyUsedRecastMesh, mOffMeshConnections); + ASSERT_TRUE(value); + ASSERT_EQ(value.get(), (NavMeshDataRef {leastRecentlyUsedData, 1})); + } + + { + const auto value = cache.get(mAgentHalfExtents, mTilePosition, mostRecentlyUsedRecastMesh, mOffMeshConnections); + ASSERT_TRUE(value); + ASSERT_EQ(value.get(), (NavMeshDataRef {mostRecentlyUsedData, 1})); + } + + const auto result = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + EXPECT_EQ(result.get(), (NavMeshDataRef {mData, 1})); + + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, leastRecentlyUsedRecastMesh, mOffMeshConnections)); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mostRecentlyUsedRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_not_replace_unused_least_recently_used_value_when_item_does_not_not_fit_cache_max_size) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh tooLargeRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto tooLargeData = reinterpret_cast(dtAlloc(2, DT_ALLOC_PERM)); + NavMeshData tooLargeNavMeshData {tooLargeData, 2}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, tooLargeRecastMesh, mOffMeshConnections, + std::move(tooLargeNavMeshData))); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_not_replace_unused_least_recently_used_value_when_item_does_not_not_fit_size_of_unused_items) + { + const std::size_t maxSize = 2; + NavMeshTilesCache cache(maxSize); + + const std::vector anotherWater {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, anotherWater, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + const std::vector tooLargeWater {1, RecastMesh::Water {2, btTransform::getIdentity()}}; + const RecastMesh tooLargeRecastMesh {mIndices, mVertices, mAreaTypes, tooLargeWater, mTrianglesPerChunk}; + const auto tooLargeData = reinterpret_cast(dtAlloc(2, DT_ALLOC_PERM)); + NavMeshData tooLargeNavMeshData {tooLargeData, 2}; + + const auto value = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + ASSERT_TRUE(value); + ASSERT_TRUE(cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData))); + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, tooLargeRecastMesh, mOffMeshConnections, + std::move(tooLargeNavMeshData))); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, release_used_after_set_then_used_by_get_item_should_left_this_item_available) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + const auto firstCopy = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + ASSERT_TRUE(firstCopy); + { + const auto secondCopy = cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections); + ASSERT_TRUE(secondCopy); + } + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData))); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, release_twice_used_item_should_left_this_item_available) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + const auto firstCopy = cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections); + ASSERT_TRUE(firstCopy); + { + const auto secondCopy = cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections); + ASSERT_TRUE(secondCopy); + } + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData))); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } +} 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..38b1ab3614 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp @@ -0,0 +1,413 @@ +#include "operators.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace DetourNavigator +{ + static inline bool operator ==(const RecastMesh::Water& lhs, const RecastMesh::Water& rhs) + { + return lhs.mCellSize == rhs.mCellSize && lhs.mTransform == rhs.mTransform; + } +} + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorRecastMeshBuilderTest : Test + { + Settings mSettings; + TileBounds mBounds; + + DetourNavigatorRecastMeshBuilderTest() + { + mSettings.mRecastScaleFactor = 1.0f; + mSettings.mTrianglesPerChunk = 256; + mBounds.mMin = osg::Vec2f(-std::numeric_limits::max() * std::numeric_limits::epsilon(), + -std::numeric_limits::max() * std::numeric_limits::epsilon()); + mBounds.mMax = osg::Vec2f(std::numeric_limits::max() * std::numeric_limits::epsilon(), + std::numeric_limits::max() * std::numeric_limits::epsilon()); + } + }; + + TEST_F(DetourNavigatorRecastMeshBuilderTest, create_for_empty_should_return_empty) + { + RecastMeshBuilder builder(mSettings, mBounds); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector()); + EXPECT_EQ(recastMesh->getIndices(), std::vector()); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector()); + } + + 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); + + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject(static_cast(shape), btTransform::getIdentity(), AreaType_ground); + const auto recastMesh = builder.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})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + 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); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)), + AreaType_ground + ); + const auto recastMesh = builder.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})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + 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); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject(static_cast(shape), btTransform::getIdentity(), AreaType_ground); + const auto recastMesh = builder.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})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground, AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_box_shape_should_produce_12_triangles) + { + btBoxShape shape(btVector3(1, 1, 2)); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject(static_cast(shape), btTransform::getIdentity(), AreaType_ground); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 2, 1, + -1, 2, 1, + 1, 2, -1, + -1, 2, -1, + 1, -2, 1, + -1, -2, 1, + 1, -2, -1, + -1, -2, -1, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({ + 0, 2, 3, + 3, 1, 0, + 0, 4, 6, + 6, 2, 0, + 0, 1, 5, + 5, 4, 0, + 7, 5, 1, + 1, 3, 7, + 7, 3, 2, + 2, 6, 7, + 7, 6, 4, + 4, 5, 7, + })); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector(12, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_compound_shape) + { + btTriangleMesh mesh1; + mesh1.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape triangle1(&mesh1, true); + btBoxShape box(btVector3(1, 1, 2)); + btTriangleMesh mesh2; + mesh2.addTriangle(btVector3(1, 1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape triangle2(&mesh2, true); + btCompoundShape shape; + shape.addChildShape(btTransform::getIdentity(), &triangle1); + shape.addChildShape(btTransform::getIdentity(), &box); + shape.addChildShape(btTransform::getIdentity(), &triangle2); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform::getIdentity(), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 0, -1, + -1, 0, 1, + -1, 0, -1, + 1, 2, 1, + -1, 2, 1, + 1, 2, -1, + -1, 2, -1, + 1, -2, 1, + -1, -2, 1, + 1, -2, -1, + -1, -2, -1, + 1, 0, -1, + -1, 0, 1, + 1, 0, 1, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({ + 0, 1, 2, + 3, 5, 6, + 6, 4, 3, + 3, 7, 9, + 9, 5, 3, + 3, 4, 8, + 8, 7, 3, + 10, 8, 4, + 4, 6, 10, + 10, 6, 5, + 5, 9, 10, + 10, 9, 7, + 7, 8, 10, + 11, 12, 13, + })); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector(14, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_transformed_compound_shape) + { + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape triangle(&mesh, true); + btCompoundShape shape; + shape.addChildShape(btTransform::getIdentity(), &triangle); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)), + AreaType_ground + ); + const auto recastMesh = builder.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})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_transformed_compound_shape_with_transformed_bhv_triangle_shape) + { + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape triangle(&mesh, true); + btCompoundShape shape; + shape.addChildShape(btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)), + &triangle); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 3, 12, 2, + 1, 12, 10, + 1, 12, 2, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, without_bounds_add_bhv_triangle_shape_should_not_filter_by_bounds) + { + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + mesh.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform::getIdentity(), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 0, -1, + -1, 0, 1, + -1, 0, -1, + -2, 0, -3, + -3, 0, -2, + -3, 0, -3, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2, 3, 4, 5})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector(2, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_bhv_triangle_shape_should_filter_by_bounds) + { + mSettings.mRecastScaleFactor = 0.1f; + mBounds.mMin = osg::Vec2f(-3, -3) * mSettings.mRecastScaleFactor; + mBounds.mMax = osg::Vec2f(-2, -2) * mSettings.mRecastScaleFactor; + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + mesh.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform::getIdentity(), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + -0.2f, 0, -0.3f, + -0.3f, 0, -0.2f, + -0.3f, 0, -0.3f, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_rotated_by_x_bhv_triangle_shape_should_filter_by_bounds) + { + mBounds.mMin = osg::Vec2f(-5, -5); + mBounds.mMax = osg::Vec2f(5, -2); + btTriangleMesh mesh; + mesh.addTriangle(btVector3(0, -1, -1), btVector3(0, -1, -1), btVector3(0, 1, -1)); + mesh.addTriangle(btVector3(0, -3, -3), btVector3(0, -3, -2), btVector3(0, -2, -3)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btQuaternion(btVector3(1, 0, 0), + static_cast(-osg::PI_4))), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 0, -0.70710659027099609375, -3.535533905029296875, + 0, 0.707107067108154296875, -3.535533905029296875, + 0, 2.384185791015625e-07, -4.24264049530029296875, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_rotated_by_y_bhv_triangle_shape_should_filter_by_bounds) + { + mBounds.mMin = osg::Vec2f(-5, -5); + mBounds.mMax = osg::Vec2f(-3, 5); + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, 0, -1), btVector3(-1, 0, 1), btVector3(1, 0, -1)); + mesh.addTriangle(btVector3(-3, 0, -3), btVector3(-3, 0, -2), btVector3(-2, 0, -3)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btQuaternion(btVector3(0, 1, 0), + static_cast(osg::PI_4))), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + -3.535533905029296875, -0.70710659027099609375, 0, + -3.535533905029296875, 0.707107067108154296875, 0, + -4.24264049530029296875, 2.384185791015625e-07, 0, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_rotated_by_z_bhv_triangle_shape_should_filter_by_bounds) + { + mBounds.mMin = osg::Vec2f(-5, -5); + mBounds.mMax = osg::Vec2f(-1, -1); + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + mesh.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btQuaternion(btVector3(0, 0, 1), + static_cast(osg::PI_4))), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 0.707107067108154296875, 0, -3.535533905029296875, + -0.70710659027099609375, 0, -3.535533905029296875, + 2.384185791015625e-07, 0, -4.24264049530029296875, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, flags_values_should_be_corresponding_to_added_objects) + { + btTriangleMesh mesh1; + mesh1.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape shape1(&mesh1, true); + btTriangleMesh mesh2; + mesh2.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0)); + btBvhTriangleMeshShape shape2(&mesh2, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape1), + btTransform::getIdentity(), + AreaType_ground + ); + builder.addObject( + static_cast(shape2), + btTransform::getIdentity(), + AreaType_null + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 0, -1, + -1, 0, 1, + -1, 0, -1, + -2, 0, -3, + -3, 0, -2, + -3, 0, -3, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2, 3, 4, 5})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground, AreaType_null})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_water_then_get_water_should_return_it) + { + RecastMeshBuilder builder(mSettings, mBounds); + builder.addWater(1000, btTransform(btMatrix3x3::getIdentity(), btVector3(100, 200, 300))); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getWater(), std::vector({ + RecastMesh::Water {1000, btTransform(btMatrix3x3::getIdentity(), btVector3(100, 200, 300))} + })); + } +} diff --git a/apps/openmw_test_suite/detournavigator/recastmeshobject.cpp b/apps/openmw_test_suite/detournavigator/recastmeshobject.cpp new file mode 100644 index 0000000000..9b30cadd7a --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/recastmeshobject.cpp @@ -0,0 +1,72 @@ +#include "operators.hpp" + +#include + +#include +#include + +#include + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorRecastMeshObjectTest : Test + { + btBoxShape mBoxShape {btVector3(1, 2, 3)}; + btCompoundShape mCompoundShape {btVector3(1, 2, 3)}; + btTransform mTransform {btQuaternion(btVector3(1, 2, 3), 1), btVector3(1, 2, 3)}; + + DetourNavigatorRecastMeshObjectTest() + { + mCompoundShape.addChildShape(mTransform, std::addressof(mBoxShape)); + } + }; + + TEST_F(DetourNavigatorRecastMeshObjectTest, constructed_object_should_have_shape_and_transform) + { + const RecastMeshObject object(mBoxShape, mTransform, AreaType_ground); + EXPECT_EQ(std::addressof(object.getShape()), std::addressof(mBoxShape)); + EXPECT_EQ(object.getTransform(), mTransform); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_with_same_transform_for_not_compound_shape_should_return_false) + { + RecastMeshObject object(mBoxShape, mTransform, AreaType_ground); + EXPECT_FALSE(object.update(mTransform, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_with_different_transform_should_return_true) + { + RecastMeshObject object(mBoxShape, mTransform, AreaType_ground); + EXPECT_TRUE(object.update(btTransform::getIdentity(), AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_with_different_flags_should_return_true) + { + RecastMeshObject object(mBoxShape, mTransform, AreaType_ground); + EXPECT_TRUE(object.update(mTransform, AreaType_null)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_for_compound_shape_with_same_transform_and_not_changed_child_transform_should_return_false) + { + RecastMeshObject object(mCompoundShape, mTransform, AreaType_ground); + EXPECT_FALSE(object.update(mTransform, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_for_compound_shape_with_same_transform_and_changed_child_transform_should_return_true) + { + RecastMeshObject object(mCompoundShape, mTransform, AreaType_ground); + mCompoundShape.updateChildTransform(0, btTransform::getIdentity()); + EXPECT_TRUE(object.update(mTransform, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, repeated_update_for_compound_shape_without_changes_should_return_false) + { + RecastMeshObject object(mCompoundShape, mTransform, AreaType_ground); + mCompoundShape.updateChildTransform(0, btTransform::getIdentity()); + object.update(mTransform, AreaType_ground); + EXPECT_FALSE(object.update(mTransform, AreaType_ground)); + } +} 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/apps/openmw_test_suite/nifloader/testbulletnifloader.cpp b/apps/openmw_test_suite/nifloader/testbulletnifloader.cpp index 2361718b19..5866b7646c 100644 --- a/apps/openmw_test_suite/nifloader/testbulletnifloader.cpp +++ b/apps/openmw_test_suite/nifloader/testbulletnifloader.cpp @@ -142,6 +142,7 @@ namespace Resource static bool operator ==(const Resource::BulletShape& lhs, const Resource::BulletShape& rhs) { return compareObjects(lhs.mCollisionShape, rhs.mCollisionShape) + && compareObjects(lhs.mAvoidCollisionShape, rhs.mAvoidCollisionShape) && lhs.mCollisionBoxHalfExtents == rhs.mCollisionBoxHalfExtents && lhs.mCollisionBoxTranslate == rhs.mCollisionBoxTranslate && lhs.mAnimatedShapes == rhs.mAnimatedShapes; @@ -151,6 +152,7 @@ namespace Resource { return stream << "Resource::BulletShape {" << value.mCollisionShape << ", " + << value.mAvoidCollisionShape << ", " << value.mCollisionBoxHalfExtents << ", " << value.mAnimatedShapes << "}"; @@ -266,7 +268,8 @@ namespace value.target = Nif::ControlledPtr(nullptr); } - void copy(const btTransform& src, Nif::Transformation& dst) { + void copy(const btTransform& src, Nif::Transformation& dst) + { dst.pos = osg::Vec3f(src.getOrigin().x(), src.getOrigin().y(), src.getOrigin().z()); for (int row = 0; row < 3; ++row) for (int column = 0; column < 3; ++column) @@ -837,7 +840,10 @@ namespace EXPECT_CALL(mNifFile, getFilename()).WillOnce(Return("test.nif")); const auto result = mLoader.load(mNifFile); + std::unique_ptr triangles(new btTriangleMesh(false)); + triangles->addTriangle(btVector3(0, 0, 0), btVector3(1, 0, 0), btVector3(1, 1, 0)); Resource::BulletShape expected; + expected.mAvoidCollisionShape = new Resource::TriangleMeshShape(triangles.release(), false); EXPECT_EQ(*result, expected); } diff --git a/appveyor.yml b/appveyor.yml index 9877d2105f..5c3522535d 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -68,7 +68,7 @@ test: off #notifications: # - provider: Email # to: -# - +# - # on_build_failure: true # on_build_status_changed: true diff --git a/cmake/FindRecastNavigation.cmake b/cmake/FindRecastNavigation.cmake new file mode 100644 index 0000000000..2126b88550 --- /dev/null +++ b/cmake/FindRecastNavigation.cmake @@ -0,0 +1,40 @@ +find_path(RecastNavigation_INCLUDE_DIR + NAMES Recast.h + HINTS $ENV{RecastNavigation_ROOT} + ${RecastNavigation_ROOT} + PATH_SUFFIXES include +) +mark_as_advanced(RecastNavigation_INCLUDE_DIR) + +include(FindPackageHandleStandardArgs) + +set(RecastNavigation_LIBRARIES "") + +foreach(COMPONENT ${RecastNavigation_FIND_COMPONENTS}) + if(NOT RecastNavigation_${COMPONENT}_FOUND) + find_library(RecastNavigation_${COMPONENT}_LIBRARY + HINTS $ENV{RecastNavigation_ROOT} + ${RecastNavigation_ROOT} + NAMES ${COMPONENT} + PATH_SUFFIXES lib + ) + find_package_handle_standard_args(RecastNavigation_${COMPONENT} DEFAULT_MSG + RecastNavigation_${COMPONENT}_LIBRARY + RecastNavigation_INCLUDE_DIR + ) + mark_as_advanced(RecastNavigation_${COMPONENT}_LIBRARY) + if(RecastNavigation_${COMPONENT}_FOUND) + list(APPEND RecastNavigation_LIBRARIES ${RecastNavigation_${COMPONENT}_LIBRARY}) + endif() + endif() +endforeach() +mark_as_advanced(RecastNavigation_LIBRARIES) + +find_package_handle_standard_args(RecastNavigation DEFAULT_MSG + RecastNavigation_LIBRARIES + RecastNavigation_INCLUDE_DIR +) + +if(RecastNavigation_FOUND) + set(RecastNavigation_INCLUDE_DIRS ${RecastNavigation_INCLUDE_DIR}) +endif() diff --git a/components/CMakeLists.txt b/components/CMakeLists.txt index 5c245afd08..e70784f4bc 100644 --- a/components/CMakeLists.txt +++ b/components/CMakeLists.txt @@ -51,7 +51,7 @@ add_component_dir (shader add_component_dir (sceneutil clone attach visitor util statesetupdater controller skeleton riggeometry morphgeometry lightcontroller lightmanager lightutil positionattitudetransform workqueue unrefqueue pathgridutil waterutil writescene serialize optimizer - actorutil + actorutil detourdebugdraw navmesh agentpath ) add_component_dir (nif @@ -156,6 +156,23 @@ if(NOT WIN32 AND NOT ANDROID) ) endif() +add_component_dir(detournavigator + debug + makenavmesh + findsmoothpath + recastmeshbuilder + recastmeshmanager + cachedrecastmeshmanager + navmeshmanager + navigator + asyncnavmeshupdater + chunkytrimesh + recastmesh + tilecachedrecastmeshmanager + recastmeshobject + navmeshtilescache + ) + set (ESM_UI ${CMAKE_SOURCE_DIR}/files/ui/contentselector.ui ) @@ -196,6 +213,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 DebugUtils Detour Recast REQUIRED) + +include_directories(SYSTEM ${RecastNavigation_INCLUDE_DIRS}) + target_link_libraries(components ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} @@ -214,6 +235,7 @@ target_link_libraries(components ${SDL2_LIBRARIES} ${OPENGL_gl_LIBRARY} ${MyGUI_LIBRARIES} + ${RecastNavigation_LIBRARIES} ) if (WIN32) diff --git a/components/bullethelpers/operators.hpp b/components/bullethelpers/operators.hpp new file mode 100644 index 0000000000..ea88deddf0 --- /dev/null +++ b/components/bullethelpers/operators.hpp @@ -0,0 +1,71 @@ +#ifndef OPENMW_COMPONENTS_BULLETHELPERS_OPERATORS_H +#define OPENMW_COMPONENTS_BULLETHELPERS_OPERATORS_H + +#include +#include +#include + +#include +#include +#include +#include + +inline std::ostream& operator <<(std::ostream& stream, const btVector3& value) +{ + return stream << "btVector3(" << std::setprecision(std::numeric_limits::max_exponent10) << value.x() + << ", " << std::setprecision(std::numeric_limits::max_exponent10) << value.y() + << ", " << std::setprecision(std::numeric_limits::max_exponent10) << value.z() + << ')'; +} + +inline std::ostream& operator <<(std::ostream& stream, BroadphaseNativeTypes value) +{ + switch (value) + { +#ifndef SHAPE_NAME +#define SHAPE_NAME(name) case name: return stream << #name; + SHAPE_NAME(BOX_SHAPE_PROXYTYPE) + SHAPE_NAME(TRIANGLE_SHAPE_PROXYTYPE) + SHAPE_NAME(TETRAHEDRAL_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_HULL_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE) + SHAPE_NAME(CUSTOM_POLYHEDRAL_SHAPE_TYPE) + SHAPE_NAME(IMPLICIT_CONVEX_SHAPES_START_HERE) + SHAPE_NAME(SPHERE_SHAPE_PROXYTYPE) + SHAPE_NAME(MULTI_SPHERE_SHAPE_PROXYTYPE) + SHAPE_NAME(CAPSULE_SHAPE_PROXYTYPE) + SHAPE_NAME(CONE_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_SHAPE_PROXYTYPE) + SHAPE_NAME(CYLINDER_SHAPE_PROXYTYPE) + SHAPE_NAME(UNIFORM_SCALING_SHAPE_PROXYTYPE) + SHAPE_NAME(MINKOWSKI_SUM_SHAPE_PROXYTYPE) + SHAPE_NAME(MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE) + SHAPE_NAME(BOX_2D_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_2D_SHAPE_PROXYTYPE) + SHAPE_NAME(CUSTOM_CONVEX_SHAPE_TYPE) + SHAPE_NAME(CONCAVE_SHAPES_START_HERE) + SHAPE_NAME(TRIANGLE_MESH_SHAPE_PROXYTYPE) + SHAPE_NAME(SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + SHAPE_NAME(FAST_CONCAVE_MESH_PROXYTYPE) + SHAPE_NAME(TERRAIN_SHAPE_PROXYTYPE) + SHAPE_NAME(GIMPACT_SHAPE_PROXYTYPE) + SHAPE_NAME(MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE) + SHAPE_NAME(EMPTY_SHAPE_PROXYTYPE) + SHAPE_NAME(STATIC_PLANE_PROXYTYPE) + SHAPE_NAME(CUSTOM_CONCAVE_SHAPE_TYPE) + SHAPE_NAME(CONCAVE_SHAPES_END_HERE) + SHAPE_NAME(COMPOUND_SHAPE_PROXYTYPE) + SHAPE_NAME(SOFTBODY_SHAPE_PROXYTYPE) + SHAPE_NAME(HFFLUID_SHAPE_PROXYTYPE) + SHAPE_NAME(HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE) + SHAPE_NAME(INVALID_SHAPE_PROXYTYPE) + SHAPE_NAME(MAX_BROADPHASE_COLLISION_TYPES) +#undef SHAPE_NAME +#endif + default: + return stream << "undefined(" << static_cast(value) << ")"; + } +} + +#endif diff --git a/components/compiler/extensions0.cpp b/components/compiler/extensions0.cpp index 54b8c03cbe..a640d76d89 100644 --- a/components/compiler/extensions0.cpp +++ b/components/compiler/extensions0.cpp @@ -320,6 +320,9 @@ namespace Compiler extensions.registerInstruction ("removefromlevitem", "ccl", opcodeRemoveFromLevItem); extensions.registerInstruction ("tb", "", opcodeToggleBorders); extensions.registerInstruction ("toggleborders", "", opcodeToggleBorders); + extensions.registerInstruction ("togglenavmesh", "", opcodeToggleNavMesh); + extensions.registerInstruction ("toggleactorspaths", "", opcodeToggleActorsPaths); + extensions.registerInstruction ("setnavmeshnumber", "l", opcodeSetNavMeshNumberToRender); } } diff --git a/components/compiler/opcodes.hpp b/components/compiler/opcodes.hpp index c141eec682..a2d8a9467c 100644 --- a/components/compiler/opcodes.hpp +++ b/components/compiler/opcodes.hpp @@ -296,6 +296,9 @@ namespace Compiler const int opcodeShowSceneGraph = 0x2002f; const int opcodeShowSceneGraphExplicit = 0x20030; const int opcodeToggleBorders = 0x2000307; + const int opcodeToggleNavMesh = 0x2000308; + const int opcodeToggleActorsPaths = 0x2000309; + const int opcodeSetNavMeshNumberToRender = 0x200030a; } namespace Sky diff --git a/components/detournavigator/areatype.hpp b/components/detournavigator/areatype.hpp new file mode 100644 index 0000000000..0daa524ca1 --- /dev/null +++ b/components/detournavigator/areatype.hpp @@ -0,0 +1,16 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_AREATYPE_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_AREATYPE_H + +#include + +namespace DetourNavigator +{ + enum AreaType : unsigned char + { + AreaType_null = RC_NULL_AREA, + AreaType_water, + AreaType_ground = RC_WALKABLE_AREA, + }; +} + +#endif diff --git a/components/detournavigator/asyncnavmeshupdater.cpp b/components/detournavigator/asyncnavmeshupdater.cpp new file mode 100644 index 0000000000..f4efc744b7 --- /dev/null +++ b/components/detournavigator/asyncnavmeshupdater.cpp @@ -0,0 +1,198 @@ +#include "asyncnavmeshupdater.hpp" +#include "debug.hpp" +#include "makenavmesh.hpp" +#include "settings.hpp" + +#include + +#include + +namespace +{ + using DetourNavigator::ChangeType; + using DetourNavigator::TilePosition; + + int getManhattanDistance(const TilePosition& lhs, const TilePosition& rhs) + { + return std::abs(lhs.x() - rhs.x()) + std::abs(lhs.y() - rhs.y()); + } + + std::tuple makePriority(const TilePosition& position, const ChangeType changeType, + const TilePosition& playerTile) + { + return std::make_tuple( + changeType, + getManhattanDistance(position, playerTile), + getManhattanDistance(position, TilePosition {0, 0}) + ); + } +} + +namespace DetourNavigator +{ + static std::ostream& operator <<(std::ostream& stream, UpdateNavMeshStatus value) + { + switch (value) + { + case UpdateNavMeshStatus::ignore: + return stream << "ignore"; + case UpdateNavMeshStatus::removed: + return stream << "removed"; + case UpdateNavMeshStatus::add: + return stream << "add"; + case UpdateNavMeshStatus::replaced: + return stream << "replaced"; + } + return stream << "unknown"; + } + + AsyncNavMeshUpdater::AsyncNavMeshUpdater(const Settings& settings, TileCachedRecastMeshManager& recastMeshManager, + OffMeshConnectionsManager& offMeshConnectionsManager) + : mSettings(settings) + , mRecastMeshManager(recastMeshManager) + , mOffMeshConnectionsManager(offMeshConnectionsManager) + , mShouldStop() + , mNavMeshTilesCache(settings.mMaxNavMeshTilesCacheSize) + { + for (std::size_t i = 0; i < mSettings.get().mAsyncNavMeshUpdaterThreads; ++i) + mThreads.emplace_back([&] { process(); }); + } + + AsyncNavMeshUpdater::~AsyncNavMeshUpdater() + { + mShouldStop = true; + std::unique_lock lock(mMutex); + mJobs = decltype(mJobs)(); + mHasJob.notify_all(); + lock.unlock(); + for (auto& thread : mThreads) + thread.join(); + } + + void AsyncNavMeshUpdater::post(const osg::Vec3f& agentHalfExtents, + const SharedNavMeshCacheItem& navMeshCacheItem, const TilePosition& playerTile, + const std::map& changedTiles) + { + *mPlayerTile.lock() = playerTile; + + if (changedTiles.empty()) + return; + + const std::lock_guard lock(mMutex); + + for (const auto& changedTile : changedTiles) + { + if (mPushed[agentHalfExtents].insert(changedTile.first).second) + mJobs.push(Job {agentHalfExtents, navMeshCacheItem, changedTile.first, + makePriority(changedTile.first, changedTile.second, playerTile)}); + } + + log("posted ", mJobs.size(), " jobs"); + + 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("stop process jobs"); + } + + void AsyncNavMeshUpdater::processJob(const Job& job) + { + log("process job for agent=", job.mAgentHalfExtents); + + const auto start = std::chrono::steady_clock::now(); + + const auto firstStart = setFirstStart(start); + + const auto recastMesh = mRecastMeshManager.get().getMesh(job.mChangedTile); + const auto playerTile = *mPlayerTile.lockConst(); + const auto offMeshConnections = mOffMeshConnectionsManager.get().get(job.mChangedTile); + + const auto status = updateNavMesh(job.mAgentHalfExtents, recastMesh.get(), job.mChangedTile, playerTile, + offMeshConnections, mSettings, job.mNavMeshCacheItem, mNavMeshTilesCache); + + const auto finish = std::chrono::steady_clock::now(); + + writeDebugFiles(job, recastMesh.get()); + + using FloatMs = std::chrono::duration; + + const auto locked = job.mNavMeshCacheItem.lockConst(); + log("cache updated for agent=", job.mAgentHalfExtents, " status=", status, + " generation=", locked->getGeneration(), + " revision=", locked->getNavMeshRevision(), + " time=", std::chrono::duration_cast(finish - start).count(), "ms", + " total_time=", std::chrono::duration_cast(finish - firstStart).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()) + { + mFirstStart.lock()->reset(); + mDone.notify_all(); + return boost::none; + } + log("got ", mJobs.size(), " jobs"); + const auto job = mJobs.top(); + mJobs.pop(); + const auto pushed = mPushed.find(job.mAgentHalfExtents); + pushed->second.erase(job.mChangedTile); + if (pushed->second.empty()) + mPushed.erase(pushed); + return job; + } + + void AsyncNavMeshUpdater::writeDebugFiles(const Job& job, const RecastMesh* recastMesh) const + { + std::string revision; + std::string recastMeshRevision; + std::string navMeshRevision; + if ((mSettings.get().mEnableWriteNavMeshToFile || mSettings.get().mEnableWriteRecastMeshToFile) + && (mSettings.get().mEnableRecastMeshFileNameRevision || mSettings.get().mEnableNavMeshFileNameRevision)) + { + revision = "." + std::to_string((std::chrono::steady_clock::now() + - std::chrono::steady_clock::time_point()).count()); + if (mSettings.get().mEnableRecastMeshFileNameRevision) + recastMeshRevision = revision; + if (mSettings.get().mEnableNavMeshFileNameRevision) + navMeshRevision = revision; + } + if (recastMesh && mSettings.get().mEnableWriteRecastMeshToFile) + writeToFile(*recastMesh, mSettings.get().mRecastMeshPathPrefix + std::to_string(job.mChangedTile.x()) + + "_" + std::to_string(job.mChangedTile.y()) + "_", recastMeshRevision); + if (mSettings.get().mEnableWriteNavMeshToFile) + writeToFile(job.mNavMeshCacheItem.lockConst()->getValue(), mSettings.get().mNavMeshPathPrefix, navMeshRevision); + } + + std::chrono::steady_clock::time_point AsyncNavMeshUpdater::setFirstStart(const std::chrono::steady_clock::time_point& value) + { + const auto locked = mFirstStart.lock(); + if (!*locked) + *locked = value; + return *locked.get(); + } +} diff --git a/components/detournavigator/asyncnavmeshupdater.hpp b/components/detournavigator/asyncnavmeshupdater.hpp new file mode 100644 index 0000000000..c9d7b1940e --- /dev/null +++ b/components/detournavigator/asyncnavmeshupdater.hpp @@ -0,0 +1,88 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H + +#include "navmeshcacheitem.hpp" +#include "offmeshconnectionsmanager.hpp" +#include "tilecachedrecastmeshmanager.hpp" +#include "tileposition.hpp" +#include "navmeshtilescache.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + enum class ChangeType + { + remove = 0, + mixed = 1, + add = 2, + }; + + class AsyncNavMeshUpdater + { + public: + AsyncNavMeshUpdater(const Settings& settings, TileCachedRecastMeshManager& recastMeshManager, + OffMeshConnectionsManager& offMeshConnectionsManager); + ~AsyncNavMeshUpdater(); + + void post(const osg::Vec3f& agentHalfExtents, const SharedNavMeshCacheItem& mNavMeshCacheItem, + const TilePosition& playerTile, const std::map& changedTiles); + + void wait(); + + private: + struct Job + { + osg::Vec3f mAgentHalfExtents; + SharedNavMeshCacheItem mNavMeshCacheItem; + TilePosition mChangedTile; + std::tuple mPriority; + + friend inline bool operator <(const Job& lhs, const Job& rhs) + { + return lhs.mPriority > rhs.mPriority; + } + }; + + using Jobs = std::priority_queue>; + + std::reference_wrapper mSettings; + std::reference_wrapper mRecastMeshManager; + std::reference_wrapper mOffMeshConnectionsManager; + std::atomic_bool mShouldStop; + std::mutex mMutex; + std::condition_variable mHasJob; + std::condition_variable mDone; + Jobs mJobs; + std::map> mPushed; + Misc::ScopeGuarded mPlayerTile; + Misc::ScopeGuarded> mFirstStart; + NavMeshTilesCache mNavMeshTilesCache; + std::vector mThreads; + + void process() throw(); + + void processJob(const Job& job); + + boost::optional getNextJob(); + + void writeDebugFiles(const Job& job, const RecastMesh* recastMesh) const; + + std::chrono::steady_clock::time_point setFirstStart(const std::chrono::steady_clock::time_point& value); + }; +} + +#endif diff --git a/components/detournavigator/bounds.hpp b/components/detournavigator/bounds.hpp new file mode 100644 index 0000000000..a31e410cb4 --- /dev/null +++ b/components/detournavigator/bounds.hpp @@ -0,0 +1,20 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_BOUNDS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_BOUNDS_H + +#include + +namespace DetourNavigator +{ + struct Bounds + { + osg::Vec3f mMin; + osg::Vec3f mMax; + }; + + inline bool isEmpty(const Bounds& value) + { + return value.mMin == value.mMax; + } +} + +#endif diff --git a/components/detournavigator/cachedrecastmeshmanager.cpp b/components/detournavigator/cachedrecastmeshmanager.cpp new file mode 100644 index 0000000000..5e145486b9 --- /dev/null +++ b/components/detournavigator/cachedrecastmeshmanager.cpp @@ -0,0 +1,63 @@ +#include "cachedrecastmeshmanager.hpp" +#include "debug.hpp" + +namespace DetourNavigator +{ + CachedRecastMeshManager::CachedRecastMeshManager(const Settings& settings, const TileBounds& bounds) + : mImpl(settings, bounds) + {} + + bool CachedRecastMeshManager::addObject(const ObjectId id, const btCollisionShape& shape, + const btTransform& transform, const AreaType areaType) + { + if (!mImpl.addObject(id, shape, transform, areaType)) + return false; + mCached.reset(); + return true; + } + + bool CachedRecastMeshManager::updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType) + { + if (!mImpl.updateObject(id, transform, areaType)) + return false; + mCached.reset(); + return true; + } + + boost::optional CachedRecastMeshManager::removeObject(const ObjectId id) + { + const auto object = mImpl.removeObject(id); + if (object) + mCached.reset(); + return object; + } + + bool CachedRecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, + const btTransform& transform) + { + if (!mImpl.addWater(cellPosition, cellSize, transform)) + return false; + mCached.reset(); + return true; + } + + boost::optional CachedRecastMeshManager::removeWater(const osg::Vec2i& cellPosition) + { + const auto water = mImpl.removeWater(cellPosition); + if (water) + mCached.reset(); + return water; + } + + std::shared_ptr CachedRecastMeshManager::getMesh() + { + if (!mCached) + mCached = mImpl.getMesh(); + return mCached; + } + + bool CachedRecastMeshManager::isEmpty() const + { + return mImpl.isEmpty(); + } +} diff --git a/components/detournavigator/cachedrecastmeshmanager.hpp b/components/detournavigator/cachedrecastmeshmanager.hpp new file mode 100644 index 0000000000..528e8dabc1 --- /dev/null +++ b/components/detournavigator/cachedrecastmeshmanager.hpp @@ -0,0 +1,36 @@ +#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, const TileBounds& bounds); + + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType); + + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform); + + boost::optional removeWater(const osg::Vec2i& cellPosition); + + boost::optional removeObject(const ObjectId id); + + std::shared_ptr getMesh(); + + bool isEmpty() const; + + 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..3a8fc34802 --- /dev/null +++ b/components/detournavigator/chunkytrimesh.cpp @@ -0,0 +1,179 @@ +#include "chunkytrimesh.hpp" +#include "exceptions.hpp" + +#include + +#include + +namespace DetourNavigator +{ + namespace + { + struct BoundsItem + { + Rect mBounds; + std::ptrdiff_t mOffset; + unsigned char mAreaTypes; + }; + + 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, const std::vector& inAreaTypes, + std::size_t& curNode, std::vector& nodes, std::size_t& curTri, + std::vector& outIndices, std::vector& outAreaTypes) + { + 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 + ); + outAreaTypes[curTri] = inAreaTypes[static_cast(items[i].mOffset)]; + 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, inAreaTypes, curNode, nodes, curTri, outIndices, outAreaTypes); + // Right + subdivide(items, isplit, imax, trisPerChunk, inIndices, inAreaTypes, curNode, nodes, curTri, outIndices, outAreaTypes); + + 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::vector& flags, 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); + mAreaTypes.resize(trianglesCount); + + // Build tree + std::vector items(trianglesCount); + + for (std::size_t i = 0; i < trianglesCount; i++) + { + auto& item = items[i]; + + item.mOffset = static_cast(i); + item.mAreaTypes = flags[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, flags, curNode, mNodes, curTri, mIndices, mAreaTypes); + + 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..c9d35f0865 --- /dev/null +++ b/components/detournavigator/chunkytrimesh.hpp @@ -0,0 +1,99 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_CHUNKYTRIMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_CHUNKYTRIMESH_H + +#include "areatype.hpp" + +#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 AreaType* const mAreaTypes; + 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, + const std::vector& flags, const 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, + mAreaTypes.data() + node.mOffset, + node.mSize + }; + } + + std::size_t getMaxTrisPerChunk() const + { + return mMaxTrisPerChunk; + } + + private: + std::vector mNodes; + std::vector mIndices; + std::vector mAreaTypes; + std::size_t mMaxTrisPerChunk; + }; +} + +#endif diff --git a/components/detournavigator/debug.cpp b/components/detournavigator/debug.cpp new file mode 100644 index 0000000000..0ddf002d90 --- /dev/null +++ b/components/detournavigator/debug.cpp @@ -0,0 +1,102 @@ +#include "debug.hpp" +#include "exceptions.hpp" +#include "recastmesh.hpp" + +#include + +#include + +namespace DetourNavigator +{ + void writeToFile(const RecastMesh& recastMesh, const std::string& pathPrefix, const std::string& revision) + { + const auto path = pathPrefix + "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& pathPrefix, const std::string& revision) + { + const int navMeshSetMagic = 'M' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'MSET'; + const int navMeshSetVersion = 1; + + struct NavMeshSetHeader + { + int magic; + int version; + int numTiles; + dtNavMeshParams params; + }; + + struct NavMeshTileHeader + { + dtTileRef tileRef; + int dataSize; + }; + + const auto path = pathPrefix + "all_tiles_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 = navMeshSetMagic; + header.version = navMeshSetVersion; + 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); + } + } +} diff --git a/components/detournavigator/debug.hpp b/components/detournavigator/debug.hpp new file mode 100644 index 0000000000..460e224350 --- /dev/null +++ b/components/detournavigator/debug.hpp @@ -0,0 +1,133 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H + +#include "tilebounds.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + inline std::ostream& operator <<(std::ostream& stream, const TileBounds& value) + { + return stream << "TileBounds {" << value.mMin << ", " << value.mMax << "}"; + } + + class RecastMesh; + + inline std::ostream& operator <<(std::ostream& stream, const std::chrono::steady_clock::time_point& value) + { + using float_s = std::chrono::duration>; + return stream << std::fixed << std::setprecision(4) + << std::chrono::duration_cast(value.time_since_epoch()).count(); + } + + struct Sink + { + virtual ~Sink() = default; + virtual void write(const std::string& text) = 0; + }; + + class FileSink final : public Sink + { + public: + FileSink(std::string path) + : mPath(std::move(path)) + { + mFile.exceptions(std::ios::failbit | std::ios::badbit); + } + + void write(const std::string& text) override + { + if (!mFile.is_open()) + { + mFile.open(mPath); + } + mFile << text << std::flush; + } + + private: + std::string mPath; + std::ofstream mFile; + }; + + class StdoutSink final : public Sink + { + public: + void write(const std::string& text) override + { + std::cout << text << std::flush; + } + }; + + class Log + { + public: + void setSink(std::unique_ptr sink) + { + *mSink.lock() = std::move(sink); + } + + bool isEnabled() + { + return bool(*mSink.lockConst()); + } + + void write(const std::string& text) + { + const auto sink = mSink.lock(); + if (*sink) + sink.get()->write(text); + } + + static Log& instance() + { + static Log value; + return value; + } + + private: + Misc::ScopeGuarded> mSink; + }; + + 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) + { + auto& log = Log::instance(); + if (!log.isEnabled()) + return; + std::ostringstream stream; + stream << '[' << std::chrono::steady_clock::now() << "] [" << std::this_thread::get_id() << "] "; + write(stream, std::forward(values) ...); + log.write(stream.str()); + } + + void writeToFile(const RecastMesh& recastMesh, const std::string& pathPrefix, const std::string& revision); + void writeToFile(const dtNavMesh& navMesh, const std::string& pathPrefix, const std::string& revision); +} + +#endif diff --git a/components/detournavigator/dtstatus.hpp b/components/detournavigator/dtstatus.hpp new file mode 100644 index 0000000000..a73d33be1e --- /dev/null +++ b/components/detournavigator/dtstatus.hpp @@ -0,0 +1,38 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H + +#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; + } +} + +#endif diff --git a/components/detournavigator/exceptions.hpp b/components/detournavigator/exceptions.hpp new file mode 100644 index 0000000000..fb31172eec --- /dev/null +++ b/components/detournavigator/exceptions.hpp @@ -0,0 +1,21 @@ +#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) {} + }; + + struct InvalidArgument : std::invalid_argument + { + InvalidArgument(const std::string& message) : std::invalid_argument(message) {} + InvalidArgument(const char* message) : std::invalid_argument(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..81b732b744 --- /dev/null +++ b/components/detournavigator/findsmoothpath.hpp @@ -0,0 +1,326 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H + +#include "dtstatus.hpp" +#include "exceptions.hpp" +#include "flags.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" +#include "debug.hpp" + +#include +#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 osg::Vec3f makeOsgVec3f(const btVector3& value) + { + return osg::Vec3f(value.x(), value.y(), value.z()); + } + + 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; + }; + + inline void initNavMeshQuery(dtNavMeshQuery& value, const dtNavMesh& navMesh, const int maxNodes) + { + const auto status = value.init(&navMesh, maxNodes); + if (!dtStatusSucceed(status)) + throw NavigatorException("Failed to init navmesh query"); + } + + struct MoveAlongSurfaceResult + { + osg::Vec3f mResultPos; + std::vector mVisited; + }; + + inline MoveAlongSurfaceResult moveAlongSurface(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef, + const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& filter, + const std::size_t maxVisitedSize) + { + MoveAlongSurfaceResult result; + result.mVisited.resize(maxVisitedSize); + int visitedNumber = 0; + const auto status = navMeshQuery.moveAlongSurface(startRef, startPos.ptr(), endPos.ptr(), + &filter, result.mResultPos.ptr(), result.mVisited.data(), &visitedNumber, static_cast(maxVisitedSize)); + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << "Failed to move along surface from " << startPos << " to " << endPos; + throw NavigatorException(message.str()); + } + assert(visitedNumber >= 0); + assert(visitedNumber <= static_cast(maxVisitedSize)); + result.mVisited.resize(static_cast(visitedNumber)); + return result; + } + + inline std::vector findPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef, + const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter, + const std::size_t maxSize) + { + int pathLen = 0; + std::vector result(maxSize); + const auto status = navMeshQuery.findPath(startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter, + result.data(), &pathLen, static_cast(maxSize)); + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << "Failed to find path over polygons from " << startRef << " to " << endRef; + throw NavigatorException(message.str()); + } + assert(pathLen >= 0); + assert(static_cast(pathLen) <= maxSize); + result.resize(static_cast(pathLen)); + return result; + } + + inline float getPolyHeight(const dtNavMeshQuery& navMeshQuery, const dtPolyRef ref, const osg::Vec3f& pos) + { + float result = 0.0f; + const auto status = navMeshQuery.getPolyHeight(ref, pos.ptr(), &result); + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << "Failed to get polygon height ref=" << ref << " pos=" << pos; + throw NavigatorException(message.str()); + } + return result; + } + + 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; + const auto result = moveAlongSurface(navMeshQuery, polygonPath.front(), iterPos, moveTgt, filter, 16); + + polygonPath = fixupCorridor(polygonPath, result.mVisited); + polygonPath = fixupShortcuts(polygonPath, navMeshQuery); + + float h = 0; + navMeshQuery.getPolyHeight(polygonPath.front(), result.mResultPos.ptr(), &h); + iterPos = result.mResultPos; + iterPos.y() = h; + + // 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; + iterPos.y() = getPolyHeight(navMeshQuery, polygonPath.front(), iterPos); + } + } + + // 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 Flags includeFlags, + const Settings& settings, OutputIterator out) + { + dtNavMeshQuery navMeshQuery; + initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes); + + dtQueryFilter queryFilter; + queryFilter.setIncludeFlags(includeFlags); + + 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("Navmesh polygon for start point is not found"); + + 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("Navmesh polygon for end polygon is not found"); + + const auto polygonPath = findPath(navMeshQuery, startRef, endRef, start, end, queryFilter, + settings.mMaxPolygonPathSize); + + 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/flags.hpp b/components/detournavigator/flags.hpp new file mode 100644 index 0000000000..df22209bbb --- /dev/null +++ b/components/detournavigator/flags.hpp @@ -0,0 +1,62 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FLAGS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FLAGS_H + +#include + +namespace DetourNavigator +{ + using Flags = unsigned short; + + enum Flag : Flags + { + Flag_none = 0, + Flag_walk = 1 << 0, + Flag_swim = 1 << 1, + Flag_openDoor = 1 << 2, + }; + + inline std::ostream& operator <<(std::ostream& stream, const Flag value) + { + switch (value) { + case Flag_none: + return stream << "none"; + case Flag_walk: + return stream << "walk"; + case Flag_swim: + return stream << "swim"; + case Flag_openDoor: + return stream << "openDoor"; + } + } + + struct WriteFlags + { + Flags mValue; + + friend inline std::ostream& operator <<(std::ostream& stream, const WriteFlags& value) + { + if (value.mValue == Flag_none) + { + return stream << Flag_none; + } + else + { + bool first = true; + for (const auto flag : {Flag_walk, Flag_swim, Flag_openDoor}) + { + if (value.mValue & flag) + { + if (!first) + stream << " | "; + first = false; + stream << flag; + } + } + + return stream; + } + } + }; +} + +#endif diff --git a/components/detournavigator/gettilespositions.hpp b/components/detournavigator/gettilespositions.hpp new file mode 100644 index 0000000000..86ce77402d --- /dev/null +++ b/components/detournavigator/gettilespositions.hpp @@ -0,0 +1,73 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_GETTILESPOSITIONS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_GETTILESPOSITIONS_H + +#include "settings.hpp" +#include "settingsutils.hpp" +#include "tileposition.hpp" + +#include + +#include + +namespace DetourNavigator +{ + inline osg::Vec3f makeOsgVec3f(const btVector3& value) + { + return osg::Vec3f(value.x(), value.y(), value.z()); + } + + template + void getTilesPositions(const osg::Vec3f& aabbMin, const osg::Vec3f& aabbMax, + const Settings& settings, Callback&& callback) + { + auto min = toNavMeshCoordinates(settings, aabbMin); + auto max = toNavMeshCoordinates(settings, aabbMax); + + const auto border = getBorderSize(settings); + min -= osg::Vec3f(border, border, border); + max += osg::Vec3f(border, border, border); + + auto minTile = getTilePosition(settings, min); + auto maxTile = getTilePosition(settings, max); + + if (minTile.x() > maxTile.x()) + std::swap(minTile.x(), maxTile.x()); + + if (minTile.y() > maxTile.y()) + std::swap(minTile.y(), maxTile.y()); + + for (int tileX = minTile.x(); tileX <= maxTile.x(); ++tileX) + for (int tileY = minTile.y(); tileY <= maxTile.y(); ++tileY) + callback(TilePosition {tileX, tileY}); + } + + template + void getTilesPositions(const btCollisionShape& shape, const btTransform& transform, + const Settings& settings, Callback&& callback) + { + btVector3 aabbMin; + btVector3 aabbMax; + shape.getAabb(transform, aabbMin, aabbMax); + + getTilesPositions(makeOsgVec3f(aabbMin), makeOsgVec3f(aabbMax), settings, std::forward(callback)); + } + + template + void getTilesPositions(const int cellSize, const btTransform& transform, + const Settings& settings, Callback&& callback) + { + const auto halfCellSize = cellSize / 2; + auto aabbMin = transform(btVector3(-halfCellSize, -halfCellSize, 0)); + auto aabbMax = transform(btVector3(halfCellSize, halfCellSize, 0)); + + aabbMin.setX(std::min(aabbMin.x(), aabbMax.x())); + aabbMin.setY(std::min(aabbMin.y(), aabbMax.y())); + + aabbMax.setX(std::max(aabbMin.x(), aabbMax.x())); + aabbMax.setY(std::max(aabbMin.y(), aabbMax.y())); + + getTilesPositions(makeOsgVec3f(aabbMin), makeOsgVec3f(aabbMax), settings, std::forward(callback)); + } +} + +#endif diff --git a/components/detournavigator/makenavmesh.cpp b/components/detournavigator/makenavmesh.cpp new file mode 100644 index 0000000000..7072f2a230 --- /dev/null +++ b/components/detournavigator/makenavmesh.cpp @@ -0,0 +1,633 @@ +#include "makenavmesh.hpp" +#include "chunkytrimesh.hpp" +#include "debug.hpp" +#include "dtstatus.hpp" +#include "exceptions.hpp" +#include "recastmesh.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" +#include "sharednavmesh.hpp" +#include "settingsutils.hpp" +#include "flags.hpp" +#include "navmeshtilescache.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ + using namespace DetourNavigator; + + static const int doNotTransferOwnership = 0; + + 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; + + osg::Vec3f makeOsgVec3f(const btVector3& value) + { + return osg::Vec3f(value.x(), value.y(), value.z()); + } + + struct WaterBounds + { + osg::Vec3f mMin; + osg::Vec3f mMax; + }; + + WaterBounds getWaterBounds(const RecastMesh::Water& water, const Settings& settings, + const osg::Vec3f& agentHalfExtents) + { + if (water.mCellSize == std::numeric_limits::max()) + { + const auto transform = getSwimLevelTransform(settings, water.mTransform, agentHalfExtents.z()); + const auto min = toNavMeshCoordinates(settings, makeOsgVec3f(transform(btVector3(-1, -1, 0)))); + const auto max = toNavMeshCoordinates(settings, makeOsgVec3f(transform(btVector3(1, 1, 0)))); + return WaterBounds { + osg::Vec3f(-std::numeric_limits::max(), min.y(), -std::numeric_limits::max()), + osg::Vec3f(std::numeric_limits::max(), max.y(), std::numeric_limits::max()) + }; + } + else + { + const auto transform = getSwimLevelTransform(settings, water.mTransform, agentHalfExtents.z()); + const auto halfCellSize = water.mCellSize / 2.0f; + return WaterBounds { + toNavMeshCoordinates(settings, makeOsgVec3f(transform(btVector3(-halfCellSize, -halfCellSize, 0)))), + toNavMeshCoordinates(settings, makeOsgVec3f(transform(btVector3(halfCellSize, halfCellSize, 0)))) + }; + } + } + + std::vector getOffMeshVerts(const std::vector& connections) + { + std::vector result; + + result.reserve(connections.size() * 6); + + const auto add = [&] (const osg::Vec3f& v) + { + result.push_back(v.x()); + result.push_back(v.y()); + result.push_back(v.z()); + }; + + for (const auto& v : connections) + { + add(v.mStart); + add(v.mEnd); + } + + return result; + } + + rcConfig makeConfig(const osg::Vec3f& agentHalfExtents, const osg::Vec3f& boundsMin, const osg::Vec3f& boundsMax, + const Settings& settings) + { + 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 = settings.mBorderSize; + 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] -= getBorderSize(settings); + config.bmin[2] -= getBorderSize(settings); + config.bmax[0] += getBorderSize(settings); + config.bmax[2] += getBorderSize(settings); + + return config; + } + + void createHeightfield(rcContext& context, rcHeightfield& solid, int width, int height, const float* bmin, + const float* bmax, const float cs, const float ch) + { + const auto result = rcCreateHeightfield(&context, solid, width, height, bmin, bmax, cs, ch); + + if (!result) + throw NavigatorException("Failed to create heightfield for navmesh"); + } + + bool rasterizeSolidObjectsTriangles(rcContext& context, const RecastMesh& recastMesh, const rcConfig& config, + rcHeightfield& solid) + { + const auto& chunkyMesh = recastMesh.getChunkyTriMesh(); + std::vector areas(chunkyMesh.getMaxTrisPerChunk(), AreaType_null); + 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 false; + + 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()), + AreaType_null + ); + + rcMarkWalkableTriangles( + &context, + config.walkableSlopeAngle, + recastMesh.getVertices().data(), + static_cast(recastMesh.getVerticesCount()), + chunk.mIndices, + static_cast(chunk.mSize), + areas.data() + ); + + for (std::size_t i = 0; i < chunk.mSize; ++i) + areas[i] = chunk.mAreaTypes[i]; + + rcClearUnwalkableTriangles( + &context, + config.walkableSlopeAngle, + recastMesh.getVertices().data(), + static_cast(recastMesh.getVerticesCount()), + chunk.mIndices, + static_cast(chunk.mSize), + areas.data() + ); + + const auto trianglesRasterized = rcRasterizeTriangles( + &context, + recastMesh.getVertices().data(), + static_cast(recastMesh.getVerticesCount()), + chunk.mIndices, + areas.data(), + static_cast(chunk.mSize), + solid, + config.walkableClimb + ); + + if (!trianglesRasterized) + throw NavigatorException("Failed to create rasterize triangles from recast mesh for navmesh"); + } + + return true; + } + + void rasterizeWaterTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, + const Settings& settings, const rcConfig& config, rcHeightfield& solid) + { + const std::array areas {{AreaType_water, AreaType_water}}; + + for (const auto& water : recastMesh.getWater()) + { + const auto bounds = getWaterBounds(water, settings, agentHalfExtents); + + const osg::Vec2f tileBoundsMin( + std::min(config.bmax[0], std::max(config.bmin[0], bounds.mMin.x())), + std::min(config.bmax[2], std::max(config.bmin[2], bounds.mMin.z())) + ); + const osg::Vec2f tileBoundsMax( + std::min(config.bmax[0], std::max(config.bmin[0], bounds.mMax.x())), + std::min(config.bmax[2], std::max(config.bmin[2], bounds.mMax.z())) + ); + + if (tileBoundsMax == tileBoundsMin) + continue; + + const std::array vertices {{ + osg::Vec3f(tileBoundsMin.x(), bounds.mMin.y(), tileBoundsMin.y()), + osg::Vec3f(tileBoundsMin.x(), bounds.mMin.y(), tileBoundsMax.y()), + osg::Vec3f(tileBoundsMax.x(), bounds.mMin.y(), tileBoundsMax.y()), + osg::Vec3f(tileBoundsMax.x(), bounds.mMin.y(), tileBoundsMin.y()), + }}; + + std::array convertedVertices; + auto convertedVerticesIt = convertedVertices.begin(); + + for (const auto& vertex : vertices) + convertedVerticesIt = std::copy(vertex.ptr(), vertex.ptr() + 3, convertedVerticesIt); + + const std::array indices {{ + 0, 1, 2, + 0, 2, 3, + }}; + + const auto trianglesRasterized = rcRasterizeTriangles( + &context, + convertedVertices.data(), + static_cast(convertedVertices.size() / 3), + indices.data(), + areas.data(), + static_cast(areas.size()), + solid, + config.walkableClimb + ); + + if (!trianglesRasterized) + throw NavigatorException("Failed to create rasterize water triangles for navmesh"); + } + } + + bool rasterizeTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, + const rcConfig& config, const Settings& settings, rcHeightfield& solid) + { + if (!rasterizeSolidObjectsTriangles(context, recastMesh, config, solid)) + return false; + + rasterizeWaterTriangles(context, agentHalfExtents, recastMesh, settings, config, solid); + + return true; + } + + void buildCompactHeightfield(rcContext& context, const int walkableHeight, const int walkableClimb, + rcHeightfield& solid, rcCompactHeightfield& compact) + { + const auto result = rcBuildCompactHeightfield(&context, walkableHeight, + walkableClimb, solid, compact); + + if (!result) + throw NavigatorException("Failed to build compact heightfield for navmesh"); + } + + void erodeWalkableArea(rcContext& context, int walkableRadius, rcCompactHeightfield& compact) + { + const auto result = rcErodeWalkableArea(&context, walkableRadius, compact); + + if (!result) + throw NavigatorException("Failed to erode walkable area for navmesh"); + } + + void buildDistanceField(rcContext& context, rcCompactHeightfield& compact) + { + const auto result = rcBuildDistanceField(&context, compact); + + if (!result) + throw NavigatorException("Failed to build distance field for navmesh"); + } + + void buildRegions(rcContext& context, rcCompactHeightfield& compact, const int borderSize, + const int minRegionArea, const int mergeRegionArea) + { + const auto result = rcBuildRegions(&context, compact, borderSize, minRegionArea, mergeRegionArea); + + if (!result) + throw NavigatorException("Failed to build distance field for navmesh"); + } + + void buildContours(rcContext& context, rcCompactHeightfield& compact, const float maxError, const int maxEdgeLen, + rcContourSet& contourSet, const int buildFlags = RC_CONTOUR_TESS_WALL_EDGES) + { + const auto result = rcBuildContours(&context, compact, maxError, maxEdgeLen, contourSet, buildFlags); + + if (!result) + throw NavigatorException("Failed to build contours for navmesh"); + } + + void buildPolyMesh(rcContext& context, rcContourSet& contourSet, const int maxVertsPerPoly, rcPolyMesh& polyMesh) + { + const auto result = rcBuildPolyMesh(&context, contourSet, maxVertsPerPoly, polyMesh); + + if (!result) + throw NavigatorException("Failed to build poly mesh for navmesh"); + } + + void buildPolyMeshDetail(rcContext& context, const rcPolyMesh& polyMesh, const rcCompactHeightfield& compact, + const float sampleDist, const float sampleMaxError, rcPolyMeshDetail& polyMeshDetail) + { + const auto result = rcBuildPolyMeshDetail(&context, polyMesh, compact, sampleDist, sampleMaxError, + polyMeshDetail); + + if (!result) + throw NavigatorException("Failed to build detail poly mesh for navmesh"); + } + + void setPolyMeshFlags(rcPolyMesh& polyMesh) + { + for (int i = 0; i < polyMesh.npolys; ++i) + { + if (polyMesh.areas[i] == AreaType_ground) + polyMesh.flags[i] = Flag_walk; + else if (polyMesh.areas[i] == AreaType_water) + polyMesh.flags[i] = Flag_swim; + } + } + + bool fillPolyMesh(rcContext& context, const rcConfig& config, rcHeightfield& solid, rcPolyMesh& polyMesh, + rcPolyMeshDetail& polyMeshDetail) + { + rcCompactHeightfield compact; + buildCompactHeightfield(context, config.walkableHeight, config.walkableClimb, solid, compact); + + erodeWalkableArea(context, config.walkableRadius, compact); + buildDistanceField(context, compact); + buildRegions(context, compact, config.borderSize, config.minRegionArea, config.mergeRegionArea); + + rcContourSet contourSet; + buildContours(context, compact, config.maxSimplificationError, config.maxEdgeLen, contourSet); + + if (contourSet.nconts == 0) + return false; + + buildPolyMesh(context, contourSet, config.maxVertsPerPoly, polyMesh); + + buildPolyMeshDetail(context, polyMesh, compact, config.detailSampleDist, config.detailSampleMaxError, + polyMeshDetail); + + setPolyMeshFlags(polyMesh); + + return true; + } + + NavMeshData makeNavMeshTileData(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, + const std::vector& offMeshConnections, const TilePosition& tile, + const osg::Vec3f& boundsMin, const osg::Vec3f& boundsMax, const Settings& settings) + { + rcContext context; + const auto config = makeConfig(agentHalfExtents, boundsMin, boundsMax, settings); + + rcHeightfield solid; + createHeightfield(context, solid, config.width, config.height, config.bmin, config.bmax, config.cs, config.ch); + + if (!rasterizeTriangles(context, agentHalfExtents, recastMesh, config, settings, solid)) + return NavMeshData(); + + 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); + if (!fillPolyMesh(context, config, solid, polyMesh, polyMeshDetail)) + return NavMeshData(); + + const auto offMeshConVerts = getOffMeshVerts(offMeshConnections); + const std::vector offMeshConRad(offMeshConnections.size(), getRadius(settings, agentHalfExtents)); + const std::vector offMeshConDir(offMeshConnections.size(), DT_OFFMESH_CON_BIDIR); + const std::vector offMeshConAreas(offMeshConnections.size(), AreaType_ground); + const std::vector offMeshConFlags(offMeshConnections.size(), Flag_openDoor); + + 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 = offMeshConVerts.data(); + params.offMeshConRad = offMeshConRad.data(); + params.offMeshConDir = offMeshConDir.data(); + params.offMeshConAreas = offMeshConAreas.data(); + params.offMeshConFlags = offMeshConFlags.data(); + params.offMeshConUserID = nullptr; + params.offMeshConCount = static_cast(offMeshConnections.size()); + 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 = tile.x(); + params.tileY = tile.y(); + params.tileLayer = 0; + + unsigned char* navMeshData; + int navMeshDataSize; + const auto navMeshDataCreated = dtCreateNavMeshData(¶ms, &navMeshData, &navMeshDataSize); + + if (!navMeshDataCreated) + throw NavigatorException("Failed to create navmesh tile data"); + + return NavMeshData(navMeshData, navMeshDataSize); + } + + UpdateNavMeshStatus makeUpdateNavMeshStatus(bool removed, bool add) + { + if (removed && add) + return UpdateNavMeshStatus::replaced; + else if (removed) + return UpdateNavMeshStatus::removed; + else if (add) + return UpdateNavMeshStatus::add; + else + return UpdateNavMeshStatus::ignore; + } + + template + unsigned long getMinValuableBitsNumber(const T value) + { + unsigned long power = 0; + while (power < sizeof(T) * 8 && (static_cast(1) << power) < value) + ++power; + return power; + } +} + +namespace DetourNavigator +{ + NavMeshPtr makeEmptyNavMesh(const Settings& settings) + { + // 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 int polysAndTilesBits = 22; + const auto polysBits = getMinValuableBitsNumber(settings.mMaxPolys); + + if (polysBits >= polysAndTilesBits) + throw InvalidArgument("Too many polygons per tile"); + + const auto tilesBits = polysAndTilesBits - polysBits; + + dtNavMeshParams params; + std::fill_n(params.orig, 3, 0.0f); + params.tileWidth = settings.mTileSize * settings.mCellSize; + params.tileHeight = settings.mTileSize * settings.mCellSize; + params.maxTiles = 1 << tilesBits; + params.maxPolys = 1 << polysBits; + + NavMeshPtr navMesh(dtAllocNavMesh(), &dtFreeNavMesh); + const auto status = navMesh->init(¶ms); + + if (!dtStatusSucceed(status)) + throw NavigatorException("Failed to init navmesh"); + + return navMesh; + } + + UpdateNavMeshStatus updateNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh* recastMesh, + const TilePosition& changedTile, const TilePosition& playerTile, + const std::vector& offMeshConnections, const Settings& settings, + const SharedNavMeshCacheItem& navMeshCacheItem, NavMeshTilesCache& navMeshTilesCache) + { + log("update NavMesh with mutiple tiles:", + " agentHeight=", std::setprecision(std::numeric_limits::max_exponent10), + getHeight(settings, agentHalfExtents), + " agentMaxClimb=", std::setprecision(std::numeric_limits::max_exponent10), + getMaxClimb(settings), + " agentRadius=", std::setprecision(std::numeric_limits::max_exponent10), + getRadius(settings, agentHalfExtents), + " changedTile=", changedTile, + " playerTile=", playerTile, + " changedTileDistance=", getDistance(changedTile, playerTile)); + + const auto params = *navMeshCacheItem.lockConst()->getValue().getParams(); + const osg::Vec3f origin(params.orig[0], params.orig[1], params.orig[2]); + + const auto x = changedTile.x(); + const auto y = changedTile.y(); + + const auto removeTile = [&] { + const auto locked = navMeshCacheItem.lock(); + auto& navMesh = locked->getValue(); + const auto tileRef = navMesh.getTileRefAt(x, y, 0); + const auto removed = dtStatusSucceed(navMesh.removeTile(tileRef, nullptr, nullptr)); + if (removed) + locked->removeUsedTile(changedTile); + return makeUpdateNavMeshStatus(removed, false); + }; + + if (!recastMesh) + { + log("ignore add tile: recastMesh is null"); + return removeTile(); + } + + auto recastMeshBounds = recastMesh->getBounds(); + + for (const auto& water : recastMesh->getWater()) + { + const auto waterBounds = getWaterBounds(water, settings, agentHalfExtents); + recastMeshBounds.mMin.y() = std::min(recastMeshBounds.mMin.y(), waterBounds.mMin.y()); + recastMeshBounds.mMax.y() = std::max(recastMeshBounds.mMax.y(), waterBounds.mMax.y()); + } + + if (isEmpty(recastMeshBounds)) + { + log("ignore add tile: recastMesh is empty"); + return removeTile(); + } + + if (!shouldAddTile(changedTile, playerTile, params.maxTiles)) + { + log("ignore add tile: too far from player"); + return removeTile(); + } + + auto cachedNavMeshData = navMeshTilesCache.get(agentHalfExtents, changedTile, *recastMesh, offMeshConnections); + + if (!cachedNavMeshData) + { + const auto tileBounds = makeTileBounds(settings, changedTile); + const osg::Vec3f tileBorderMin(tileBounds.mMin.x(), recastMeshBounds.mMin.y() - 1, tileBounds.mMin.y()); + const osg::Vec3f tileBorderMax(tileBounds.mMax.x(), recastMeshBounds.mMax.y() + 1, tileBounds.mMax.y()); + + auto navMeshData = makeNavMeshTileData(agentHalfExtents, *recastMesh, offMeshConnections, changedTile, + tileBorderMin, tileBorderMax, settings); + + if (!navMeshData.mValue) + { + log("ignore add tile: NavMeshData is null"); + return removeTile(); + } + + try + { + cachedNavMeshData = navMeshTilesCache.set(agentHalfExtents, changedTile, *recastMesh, + offMeshConnections, std::move(navMeshData)); + } + catch (const InvalidArgument&) + { + cachedNavMeshData = navMeshTilesCache.get(agentHalfExtents, changedTile, *recastMesh, + offMeshConnections); + } + + if (!cachedNavMeshData) + { + log("cache overflow"); + + const auto locked = navMeshCacheItem.lock(); + auto& navMesh = locked->getValue(); + const auto tileRef = navMesh.getTileRefAt(x, y, 0); + const auto removed = dtStatusSucceed(navMesh.removeTile(tileRef, nullptr, nullptr)); + const auto addStatus = navMesh.addTile(navMeshData.mValue.get(), navMeshData.mSize, + doNotTransferOwnership, 0, 0); + + if (dtStatusSucceed(addStatus)) + { + locked->setUsedTile(changedTile, std::move(navMeshData)); + return makeUpdateNavMeshStatus(removed, true); + } + else + { + if (removed) + locked->removeUsedTile(changedTile); + log("failed to add tile with status=", WriteDtStatus {addStatus}); + return makeUpdateNavMeshStatus(removed, false); + } + } + } + + const auto locked = navMeshCacheItem.lock(); + auto& navMesh = locked->getValue(); + const auto tileRef = navMesh.getTileRefAt(x, y, 0); + const auto removed = dtStatusSucceed(navMesh.removeTile(tileRef, nullptr, nullptr)); + const auto addStatus = navMesh.addTile(cachedNavMeshData.get().mValue, cachedNavMeshData.get().mSize, + doNotTransferOwnership, 0, 0); + + if (dtStatusSucceed(addStatus)) + { + locked->setUsedTile(changedTile, std::move(cachedNavMeshData)); + return makeUpdateNavMeshStatus(removed, true); + } + else + { + if (removed) + locked->removeUsedTile(changedTile); + log("failed to add tile with status=", WriteDtStatus {addStatus}); + return makeUpdateNavMeshStatus(removed, false); + } + } +} diff --git a/components/detournavigator/makenavmesh.hpp b/components/detournavigator/makenavmesh.hpp new file mode 100644 index 0000000000..55d3e261c5 --- /dev/null +++ b/components/detournavigator/makenavmesh.hpp @@ -0,0 +1,55 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H + +#include "offmeshconnectionsmanager.hpp" +#include "settings.hpp" +#include "navmeshcacheitem.hpp" +#include "tileposition.hpp" +#include "tilebounds.hpp" +#include "sharednavmesh.hpp" +#include "navmeshtilescache.hpp" + +#include + +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + class RecastMesh; + struct Settings; + + enum class UpdateNavMeshStatus + { + ignore, + removed, + add, + replaced + }; + + inline float getLength(const osg::Vec2i& value) + { + return std::sqrt(float(osg::square(value.x()) + osg::square(value.y()))); + } + + inline float getDistance(const TilePosition& lhs, const TilePosition& rhs) + { + return getLength(lhs - rhs); + } + + inline bool shouldAddTile(const TilePosition& changedTile, const TilePosition& playerTile, int maxTiles) + { + const auto expectedTilesCount = std::ceil(osg::PI * osg::square(getDistance(changedTile, playerTile))); + return expectedTilesCount * 3 <= maxTiles; + } + + NavMeshPtr makeEmptyNavMesh(const Settings& settings); + + UpdateNavMeshStatus updateNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh* recastMesh, + const TilePosition& changedTile, const TilePosition& playerTile, + const std::vector& offMeshConnections, const Settings& settings, + const SharedNavMeshCacheItem& navMeshCacheItem, NavMeshTilesCache& navMeshTilesCache); +} + +#endif diff --git a/components/detournavigator/navigator.cpp b/components/detournavigator/navigator.cpp new file mode 100644 index 0000000000..73537ff6f6 --- /dev/null +++ b/components/detournavigator/navigator.cpp @@ -0,0 +1,154 @@ +#include "navigator.hpp" +#include "debug.hpp" +#include "settingsutils.hpp" + +#include + +namespace DetourNavigator +{ + Navigator::Navigator(const Settings& settings) + : mSettings(settings) + , mNavMeshManager(mSettings) + { + } + + void Navigator::addAgent(const osg::Vec3f& agentHalfExtents) + { + ++mAgents[agentHalfExtents]; + mNavMeshManager.addAgent(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::addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform) + { + return mNavMeshManager.addObject(id, shape, transform, AreaType_ground); + } + + bool Navigator::addObject(const ObjectId id, const ObjectShapes& shapes, const btTransform& transform) + { + bool result = addObject(id, shapes.mShape, transform); + if (shapes.mAvoid) + { + const ObjectId avoidId(shapes.mAvoid); + if (mNavMeshManager.addObject(avoidId, *shapes.mAvoid, transform, AreaType_null)) + { + updateAvoidShapeId(id, avoidId); + result = true; + } + } + return result; + } + + bool Navigator::addObject(const ObjectId id, const DoorShapes& shapes, const btTransform& transform) + { + if (addObject(id, static_cast(shapes), transform)) + { + mNavMeshManager.addOffMeshConnection( + id, + toNavMeshCoordinates(mSettings, shapes.mConnectionStart), + toNavMeshCoordinates(mSettings, shapes.mConnectionEnd) + ); + return true; + } + return false; + } + + bool Navigator::updateObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform) + { + return mNavMeshManager.updateObject(id, shape, transform, AreaType_ground); + } + + bool Navigator::updateObject(const ObjectId id, const ObjectShapes& shapes, const btTransform& transform) + { + bool result = updateObject(id, shapes.mShape, transform); + if (shapes.mAvoid) + { + const ObjectId avoidId(shapes.mAvoid); + if (mNavMeshManager.updateObject(avoidId, *shapes.mAvoid, transform, AreaType_null)) + { + updateAvoidShapeId(id, avoidId); + result = true; + } + } + return result; + } + + bool Navigator::updateObject(const ObjectId id, const DoorShapes& shapes, const btTransform& transform) + { + return updateObject(id, static_cast(shapes), transform); + } + + bool Navigator::removeObject(const ObjectId id) + { + bool result = mNavMeshManager.removeObject(id); + const auto avoid = mAvoidIds.find(id); + if (avoid != mAvoidIds.end()) + result = mNavMeshManager.removeObject(avoid->second) || result; + const auto water = mWaterIds.find(id); + if (water != mWaterIds.end()) + result = mNavMeshManager.removeObject(water->second) || result; + mNavMeshManager.removeOffMeshConnection(id); + return result; + } + + bool Navigator::addWater(const osg::Vec2i& cellPosition, const int cellSize, const btScalar level, + const btTransform& transform) + { + return mNavMeshManager.addWater(cellPosition, cellSize, + btTransform(transform.getBasis(), btVector3(transform.getOrigin().x(), transform.getOrigin().y(), level))); + } + + bool Navigator::removeWater(const osg::Vec2i& cellPosition) + { + return mNavMeshManager.removeWater(cellPosition); + } + + void Navigator::update(const osg::Vec3f& playerPosition) + { + for (const auto& v : mAgents) + mNavMeshManager.update(playerPosition, v.first); + } + + void Navigator::wait() + { + mNavMeshManager.wait(); + } + + std::map Navigator::getNavMeshes() const + { + return mNavMeshManager.getNavMeshes(); + } + + const Settings& Navigator::getSettings() const + { + return mSettings; + } + + void Navigator::updateAvoidShapeId(const ObjectId id, const ObjectId avoidId) + { + updateId(id, avoidId, mWaterIds); + } + + void Navigator::updateWaterShapeId(const ObjectId id, const ObjectId waterId) + { + updateId(id, waterId, mWaterIds); + } + + void Navigator::updateId(const ObjectId id, const ObjectId updateId, std::unordered_map& ids) + { + auto inserted = ids.insert(std::make_pair(id, updateId)); + if (!inserted.second) + { + mNavMeshManager.removeObject(inserted.first->second); + inserted.first->second = updateId; + } + } +} diff --git a/components/detournavigator/navigator.hpp b/components/detournavigator/navigator.hpp new file mode 100644 index 0000000000..351e0f9f84 --- /dev/null +++ b/components/detournavigator/navigator.hpp @@ -0,0 +1,205 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H + +#include "findsmoothpath.hpp" +#include "flags.hpp" +#include "navmeshmanager.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" + +namespace DetourNavigator +{ + struct ObjectShapes + { + const btCollisionShape& mShape; + const btCollisionShape* mAvoid; + + ObjectShapes(const btCollisionShape& shape, const btCollisionShape* avoid) + : mShape(shape), mAvoid(avoid) + {} + }; + + struct DoorShapes : ObjectShapes + { + osg::Vec3f mConnectionStart; + osg::Vec3f mConnectionEnd; + + DoorShapes(const btCollisionShape& shape, const btCollisionShape* avoid, + const osg::Vec3f& connectionStart,const osg::Vec3f& connectionEnd) + : ObjectShapes(shape, avoid) + , mConnectionStart(connectionStart) + , mConnectionEnd(connectionEnd) + {} + }; + + /** + * @brief Top level class of detournavigator componenet. Navigator allows to build a scene with navmesh and find + * a path for an agent there. Scene contains agents, geometry objects and water. Agent are distinguished only by + * half extents. Each object has unique identifier and could be added, updated or removed. Water could be added once + * for each world cell at given level of height. Navmesh builds asynchronously in separate threads. To start build + * navmesh call update method. + */ + class Navigator + { + public: + /** + * @brief Navigator constructor initializes all internal data. Constructed object is ready to build a scene. + * @param settings allows to customize navigator work. Constructor is only place to set navigator settings. + */ + Navigator(const Settings& settings); + + /** + * @brief addAgent should be called for each agent even if all of them has same half extents. + * @param agentHalfExtents allows to setup bounding cylinder for each agent, for each different half extents + * there is different navmesh. + */ + void addAgent(const osg::Vec3f& agentHalfExtents); + + /** + * @brief removeAgent should be called for each agent even if all of them has same half extents + * @param agentHalfExtents allows determine which agent to remove + */ + void removeAgent(const osg::Vec3f& agentHalfExtents); + + /** + * @brief addObject is used to add object represented by single btCollisionShape and btTransform. + * @param id is used to distinguish different objects. + * @param shape must live until object is updated by another shape removed from Navigator. + * @param transform allows to setup object geometry according to its world state. + * @return true if object is added, false if there is already object with given id. + */ + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform); + + /** + * @brief addObject is used to add complex object with allowed to walk and avoided to walk shapes + * @param id is used to distinguish different objects + * @param shape members must live until object is updated by another shape removed from Navigator + * @param transform allows to setup objects geometry according to its world state + * @return true if object is added, false if there is already object with given id + */ + bool addObject(const ObjectId id, const ObjectShapes& shapes, const btTransform& transform); + + /** + * @brief addObject is used to add doors. + * @param id is used to distinguish different objects. + * @param shape members must live until object is updated by another shape or removed from Navigator. + * @param transform allows to setup objects geometry according to its world state. + * @return true if object is added, false if there is already object with given id. + */ + bool addObject(const ObjectId id, const DoorShapes& shapes, const btTransform& transform); + + /** + * @brief updateObject replace object geometry by given data. + * @param id is used to find object. + * @param shape must live until object is updated by another shape removed from Navigator. + * @param transform allows to setup objects geometry according to its world state. + * @return true if object is updated, false if there is no object with given id. + */ + bool updateObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform); + + /** + * @brief updateObject replace object geometry by given data. + * @param id is used to find object. + * @param shape members must live until object is updated by another shape removed from Navigator. + * @param transform allows to setup objects geometry according to its world state. + * @return true if object is updated, false if there is no object with given id. + */ + bool updateObject(const ObjectId id, const ObjectShapes& shapes, const btTransform& transform); + + /** + * @brief updateObject replace object geometry by given data. + * @param id is used to find object. + * @param shape members must live until object is updated by another shape removed from Navigator. + * @param transform allows to setup objects geometry according to its world state. + * @return true if object is updated, false if there is no object with given id. + */ + bool updateObject(const ObjectId id, const DoorShapes& shapes, const btTransform& transform); + + /** + * @brief removeObject to make it no more available at the scene. + * @param id is used to find object. + * @return true if object is removed, false if there is no object with given id. + */ + bool removeObject(const ObjectId id); + + /** + * @brief addWater is used to set water level at given world cell. + * @param cellPosition allows to distinguish cells if there is many in current world. + * @param cellSize set cell borders. std::numeric_limits::max() disables cell borders. + * @param level set z coordinate of water surface at the scene. + * @param transform set global shift of cell center. + * @return true if there was no water at given cell if cellSize != std::numeric_limits::max() or there is + * at least single object is added to the scene, false if there is already water for given cell or there is no + * any other objects. + */ + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btScalar level, + const btTransform& transform); + + /** + * @brief removeWater to make it no more available at the scene. + * @param cellPosition allows to find cell. + * @return true if there was water at given cell. + */ + bool removeWater(const osg::Vec2i& cellPosition); + + /** + * @brief update start background navmesh update using current scene state. + * @param playerPosition setup initial point to order build tiles of navmesh. + */ + void update(const osg::Vec3f& playerPosition); + + /** + * @brief wait locks thread until all tiles are updated from last update call. + */ + void wait(); + + /** + * @brief findPath fills output iterator with points of scene surfaces to be used for actor to walk through. + * @param agentHalfExtents allows to find navmesh for given actor. + * @param start path from given point. + * @param end path at given point. + * @param includeFlags setup allowed surfaces for actor to walk. + * @param out the beginning of the destination range. + * @return Output iterator to the element in the destination range, one past the last element of found path. + * Equal to out if no path is found. + * @throws InvalidArgument if there is no navmesh for given agentHalfExtents. + */ + template + OutputIterator findPath(const osg::Vec3f& agentHalfExtents, const osg::Vec3f& start, + const osg::Vec3f& end, const Flags includeFlags, OutputIterator out) const + { + static_assert( + std::is_same< + typename std::iterator_traits::iterator_category, + std::output_iterator_tag + >::value, + "out is not an OutputIterator" + ); + const auto navMesh = mNavMeshManager.getNavMesh(agentHalfExtents); + return findSmoothPath(navMesh.lock()->getValue(), toNavMeshCoordinates(mSettings, agentHalfExtents), + toNavMeshCoordinates(mSettings, start), toNavMeshCoordinates(mSettings, end), includeFlags, + mSettings, out); + } + + /** + * @brief getNavMeshes returns all current navmeshes + * @return map of agent half extents to navmesh + */ + std::map getNavMeshes() const; + + const Settings& getSettings() const; + + private: + Settings mSettings; + NavMeshManager mNavMeshManager; + std::map mAgents; + std::unordered_map mAvoidIds; + std::unordered_map mWaterIds; + + void updateAvoidShapeId(const ObjectId id, const ObjectId avoidId); + void updateWaterShapeId(const ObjectId id, const ObjectId waterId); + void updateId(const ObjectId id, const ObjectId waterId, std::unordered_map& ids); + }; +} + +#endif diff --git a/components/detournavigator/navmeshcacheitem.hpp b/components/detournavigator/navmeshcacheitem.hpp new file mode 100644 index 0000000000..e64b9d1383 --- /dev/null +++ b/components/detournavigator/navmeshcacheitem.hpp @@ -0,0 +1,70 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHCACHEITEM_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHCACHEITEM_H + +#include "sharednavmesh.hpp" +#include "tileposition.hpp" +#include "navmeshtilescache.hpp" + +#include + +#include + +namespace DetourNavigator +{ + class NavMeshCacheItem + { + public: + NavMeshCacheItem(const NavMeshPtr& value, std::size_t generation) + : mValue(value), mGeneration(generation), mNavMeshRevision(0) + { + } + + const dtNavMesh& getValue() const + { + return *mValue; + } + + dtNavMesh& getValue() + { + return *mValue; + } + + std::size_t getGeneration() const + { + return mGeneration; + } + + std::size_t getNavMeshRevision() const + { + return mNavMeshRevision; + } + + void setUsedTile(const TilePosition& tilePosition, NavMeshTilesCache::Value value) + { + mUsedTiles[tilePosition] = std::make_pair(std::move(value), NavMeshData()); + ++mNavMeshRevision; + } + + void setUsedTile(const TilePosition& tilePosition, NavMeshData value) + { + mUsedTiles[tilePosition] = std::make_pair(NavMeshTilesCache::Value(), std::move(value)); + ++mNavMeshRevision; + } + + void removeUsedTile(const TilePosition& tilePosition) + { + mUsedTiles.erase(tilePosition); + ++mNavMeshRevision; + } + + private: + NavMeshPtr mValue; + std::size_t mGeneration; + std::size_t mNavMeshRevision; + std::map> mUsedTiles; + }; + + using SharedNavMeshCacheItem = Misc::SharedGuarded; +} + +#endif diff --git a/components/detournavigator/navmeshdata.hpp b/components/detournavigator/navmeshdata.hpp new file mode 100644 index 0000000000..8ce79614b3 --- /dev/null +++ b/components/detournavigator/navmeshdata.hpp @@ -0,0 +1,35 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHDATA_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHDATA_H + +#include + +#include +#include + +namespace DetourNavigator +{ + 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) + {} + }; +} + +#endif diff --git a/components/detournavigator/navmeshmanager.cpp b/components/detournavigator/navmeshmanager.cpp new file mode 100644 index 0000000000..9afe105d9f --- /dev/null +++ b/components/detournavigator/navmeshmanager.cpp @@ -0,0 +1,230 @@ +#include "navmeshmanager.hpp" +#include "debug.hpp" +#include "exceptions.hpp" +#include "gettilespositions.hpp" +#include "makenavmesh.hpp" +#include "navmeshcacheitem.hpp" +#include "settings.hpp" +#include "sharednavmesh.hpp" + +#include + +#include + +#include + +namespace +{ + using DetourNavigator::ChangeType; + + ChangeType addChangeType(const ChangeType current, const ChangeType add) + { + return current == add ? current : ChangeType::mixed; + } +} + +namespace DetourNavigator +{ + NavMeshManager::NavMeshManager(const Settings& settings) + : mSettings(settings) + , mRecastMeshManager(settings) + , mOffMeshConnectionsManager(settings) + , mAsyncNavMeshUpdater(settings, mRecastMeshManager, mOffMeshConnectionsManager) + {} + + bool NavMeshManager::addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + { + if (!mRecastMeshManager.addObject(id, shape, transform, areaType)) + return false; + addChangedTiles(shape, transform, ChangeType::add); + return true; + } + + bool NavMeshManager::updateObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + { + if (!mRecastMeshManager.updateObject(id, transform, areaType)) + return false; + addChangedTiles(shape, transform, ChangeType::mixed); + return true; + } + + bool NavMeshManager::removeObject(const ObjectId id) + { + const auto object = mRecastMeshManager.removeObject(id); + if (!object) + return false; + addChangedTiles(object->mShape, object->mTransform, ChangeType::remove); + return true; + } + + bool NavMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform) + { + if (!mRecastMeshManager.addWater(cellPosition, cellSize, transform)) + return false; + addChangedTiles(cellSize, transform, ChangeType::add); + return true; + } + + bool NavMeshManager::removeWater(const osg::Vec2i& cellPosition) + { + const auto water = mRecastMeshManager.removeWater(cellPosition); + if (!water) + return false; + addChangedTiles(water->mCellSize, water->mTransform, ChangeType::remove); + return true; + } + + void NavMeshManager::addAgent(const osg::Vec3f& agentHalfExtents) + { + auto cached = mCache.find(agentHalfExtents); + if (cached != mCache.end()) + return; + mCache.insert(std::make_pair(agentHalfExtents, + std::make_shared(makeEmptyNavMesh(mSettings), ++mGenerationCounter))); + log("cache add for agent=", agentHalfExtents); + } + + void NavMeshManager::reset(const osg::Vec3f& agentHalfExtents) + { + mCache.erase(agentHalfExtents); + } + + void NavMeshManager::addOffMeshConnection(const ObjectId id, const osg::Vec3f& start, const osg::Vec3f& end) + { + if (!mOffMeshConnectionsManager.add(id, OffMeshConnection {start, end})) + return; + + const auto startTilePosition = getTilePosition(mSettings, start); + const auto endTilePosition = getTilePosition(mSettings, end); + + addChangedTile(startTilePosition, ChangeType::add); + + if (startTilePosition != endTilePosition) + addChangedTile(endTilePosition, ChangeType::add); + } + + void NavMeshManager::removeOffMeshConnection(const ObjectId id) + { + if (const auto connection = mOffMeshConnectionsManager.remove(id)) + { + const auto startTilePosition = getTilePosition(mSettings, connection->mStart); + const auto endTilePosition = getTilePosition(mSettings, connection->mEnd); + + addChangedTile(startTilePosition, ChangeType::remove); + + if (startTilePosition != endTilePosition) + addChangedTile(endTilePosition, ChangeType::remove); + } + } + + void NavMeshManager::update(osg::Vec3f playerPosition, const osg::Vec3f& agentHalfExtents) + { + const auto playerTile = getTilePosition(mSettings, toNavMeshCoordinates(mSettings, playerPosition)); + auto& lastRevision = mLastRecastMeshManagerRevision[agentHalfExtents]; + auto lastPlayerTile = mPlayerTile.find(agentHalfExtents); + if (lastRevision >= mRecastMeshManager.getRevision() && lastPlayerTile != mPlayerTile.end() + && lastPlayerTile->second == playerTile) + return; + lastRevision = mRecastMeshManager.getRevision(); + if (lastPlayerTile == mPlayerTile.end()) + lastPlayerTile = mPlayerTile.insert(std::make_pair(agentHalfExtents, playerTile)).first; + else + lastPlayerTile->second = playerTile; + std::map tilesToPost; + const auto& cached = getCached(agentHalfExtents); + const auto changedTiles = mChangedTiles.find(agentHalfExtents); + { + const auto locked = cached.lock(); + const auto& navMesh = locked->getValue(); + if (changedTiles != mChangedTiles.end()) + { + for (const auto& tile : changedTiles->second) + if (navMesh.getTileAt(tile.first.x(), tile.first.y(), 0)) + { + auto tileToPost = tilesToPost.find(tile.first); + if (tileToPost == tilesToPost.end()) + tilesToPost.insert(tile); + else + tileToPost->second = addChangeType(tileToPost->second, tile.second); + } + for (const auto& tile : tilesToPost) + changedTiles->second.erase(tile.first); + if (changedTiles->second.empty()) + mChangedTiles.erase(changedTiles); + } + const auto maxTiles = navMesh.getParams()->maxTiles; + mRecastMeshManager.forEachTilePosition([&] (const TilePosition& tile) + { + if (tilesToPost.count(tile)) + return; + const auto shouldAdd = shouldAddTile(tile, playerTile, maxTiles); + const auto presentInNavMesh = bool(navMesh.getTileAt(tile.x(), tile.y(), 0)); + if (shouldAdd && !presentInNavMesh) + tilesToPost.insert(std::make_pair(tile, ChangeType::add)); + else if (!shouldAdd && presentInNavMesh) + tilesToPost.insert(std::make_pair(tile, ChangeType::mixed)); + }); + } + mAsyncNavMeshUpdater.post(agentHalfExtents, cached, playerTile, tilesToPost); + log("cache update posted for agent=", agentHalfExtents, + " playerTile=", lastPlayerTile->second, + " recastMeshManagerRevision=", lastRevision); + } + + void NavMeshManager::wait() + { + mAsyncNavMeshUpdater.wait(); + } + + SharedNavMeshCacheItem NavMeshManager::getNavMesh(const osg::Vec3f& agentHalfExtents) const + { + return getCached(agentHalfExtents); + } + + std::map NavMeshManager::getNavMeshes() const + { + return mCache; + } + + void NavMeshManager::addChangedTiles(const btCollisionShape& shape, const btTransform& transform, + const ChangeType changeType) + { + getTilesPositions(shape, transform, mSettings, + [&] (const TilePosition& v) { addChangedTile(v, changeType); }); + } + + void NavMeshManager::addChangedTiles(const int cellSize, const btTransform& transform, + const ChangeType changeType) + { + if (cellSize == std::numeric_limits::max()) + return; + + getTilesPositions(cellSize, transform, mSettings, + [&] (const TilePosition& v) { addChangedTile(v, changeType); }); + } + + void NavMeshManager::addChangedTile(const TilePosition& tilePosition, const ChangeType changeType) + { + for (const auto& cached : mCache) + { + auto& tiles = mChangedTiles[cached.first]; + auto tile = tiles.find(tilePosition); + if (tile == tiles.end()) + tiles.insert(std::make_pair(tilePosition, changeType)); + else + tile->second = addChangeType(tile->second, changeType); + } + } + + const SharedNavMeshCacheItem& NavMeshManager::getCached(const osg::Vec3f& agentHalfExtents) const + { + const auto cached = mCache.find(agentHalfExtents); + if (cached != mCache.end()) + return cached->second; + std::ostringstream stream; + stream << "Agent with half extents is not found: " << agentHalfExtents; + throw InvalidArgument(stream.str()); + } +} diff --git a/components/detournavigator/navmeshmanager.hpp b/components/detournavigator/navmeshmanager.hpp new file mode 100644 index 0000000000..f44cdd2515 --- /dev/null +++ b/components/detournavigator/navmeshmanager.hpp @@ -0,0 +1,74 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHMANAGER_H + +#include "asyncnavmeshupdater.hpp" +#include "cachedrecastmeshmanager.hpp" +#include "offmeshconnectionsmanager.hpp" +#include "sharednavmesh.hpp" + +#include + +#include + +#include +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + class NavMeshManager + { + public: + NavMeshManager(const Settings& settings); + + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool updateObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool removeObject(const ObjectId id); + + void addAgent(const osg::Vec3f& agentHalfExtents); + + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform); + + bool removeWater(const osg::Vec2i& cellPosition); + + void reset(const osg::Vec3f& agentHalfExtents); + + void addOffMeshConnection(const ObjectId id, const osg::Vec3f& start, const osg::Vec3f& end); + + void removeOffMeshConnection(const ObjectId id); + + void update(osg::Vec3f playerPosition, const osg::Vec3f& agentHalfExtents); + + void wait(); + + SharedNavMeshCacheItem getNavMesh(const osg::Vec3f& agentHalfExtents) const; + + std::map getNavMeshes() const; + + private: + const Settings& mSettings; + TileCachedRecastMeshManager mRecastMeshManager; + OffMeshConnectionsManager mOffMeshConnectionsManager; + AsyncNavMeshUpdater mAsyncNavMeshUpdater; + std::map mCache; + std::map> mChangedTiles; + std::size_t mGenerationCounter = 0; + std::map mPlayerTile; + std::map mLastRecastMeshManagerRevision; + + void addChangedTiles(const btCollisionShape& shape, const btTransform& transform, const ChangeType changeType); + + void addChangedTiles(const int cellSize, const btTransform& transform, const ChangeType changeType); + + void addChangedTile(const TilePosition& tilePosition, const ChangeType changeType); + + const SharedNavMeshCacheItem& getCached(const osg::Vec3f& agentHalfExtents) const; + }; +} + +#endif diff --git a/components/detournavigator/navmeshtilescache.cpp b/components/detournavigator/navmeshtilescache.cpp new file mode 100644 index 0000000000..8c5fc63815 --- /dev/null +++ b/components/detournavigator/navmeshtilescache.cpp @@ -0,0 +1,158 @@ +#include "navmeshtilescache.hpp" +#include "exceptions.hpp" + +namespace DetourNavigator +{ + namespace + { + inline std::string makeNavMeshKey(const RecastMesh& recastMesh, + const std::vector& offMeshConnections) + { + std::string result; + result.reserve( + recastMesh.getIndices().size() * sizeof(int) + + recastMesh.getVertices().size() * sizeof(float) + + recastMesh.getAreaTypes().size() * sizeof(AreaType) + + recastMesh.getWater().size() * sizeof(RecastMesh::Water) + + offMeshConnections.size() * sizeof(OffMeshConnection) + ); + std::copy( + reinterpret_cast(recastMesh.getIndices().data()), + reinterpret_cast(recastMesh.getIndices().data() + recastMesh.getIndices().size()), + std::back_inserter(result) + ); + std::copy( + reinterpret_cast(recastMesh.getVertices().data()), + reinterpret_cast(recastMesh.getVertices().data() + recastMesh.getVertices().size()), + std::back_inserter(result) + ); + std::copy( + reinterpret_cast(recastMesh.getAreaTypes().data()), + reinterpret_cast(recastMesh.getAreaTypes().data() + recastMesh.getAreaTypes().size()), + std::back_inserter(result) + ); + std::copy( + reinterpret_cast(recastMesh.getWater().data()), + reinterpret_cast(recastMesh.getWater().data() + recastMesh.getWater().size()), + std::back_inserter(result) + ); + std::copy( + reinterpret_cast(offMeshConnections.data()), + reinterpret_cast(offMeshConnections.data() + offMeshConnections.size()), + std::back_inserter(result) + ); + return result; + } + } + + NavMeshTilesCache::NavMeshTilesCache(const std::size_t maxNavMeshDataSize) + : mMaxNavMeshDataSize(maxNavMeshDataSize), mUsedNavMeshDataSize(0), mFreeNavMeshDataSize(0) {} + + NavMeshTilesCache::Value NavMeshTilesCache::get(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, + const RecastMesh& recastMesh, const std::vector& offMeshConnections) + { + const std::lock_guard lock(mMutex); + + const auto agentValues = mValues.find(agentHalfExtents); + if (agentValues == mValues.end()) + return Value(); + + const auto tileValues = agentValues->second.find(changedTile); + if (tileValues == agentValues->second.end()) + return Value(); + + const auto tile = tileValues->second.find(makeNavMeshKey(recastMesh, offMeshConnections)); + if (tile == tileValues->second.end()) + return Value(); + + acquireItemUnsafe(tile->second); + + return Value(*this, tile->second); + } + + NavMeshTilesCache::Value NavMeshTilesCache::set(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, + const RecastMesh& recastMesh, const std::vector& offMeshConnections, + NavMeshData&& value) + { + const auto navMeshSize = static_cast(value.mSize); + + const std::lock_guard lock(mMutex); + + if (navMeshSize > mMaxNavMeshDataSize) + return Value(); + + if (navMeshSize > mFreeNavMeshDataSize + (mMaxNavMeshDataSize - mUsedNavMeshDataSize)) + return Value(); + + while (!mFreeItems.empty() && mUsedNavMeshDataSize + navMeshSize > mMaxNavMeshDataSize) + removeLeastRecentlyUsed(); + + const auto navMeshKey = makeNavMeshKey(recastMesh, offMeshConnections); + const auto iterator = mFreeItems.emplace(mFreeItems.end(), agentHalfExtents, changedTile, navMeshKey); + const auto emplaced = mValues[agentHalfExtents][changedTile].emplace(navMeshKey, iterator); + + if (!emplaced.second) + { + mFreeItems.erase(iterator); + throw InvalidArgument("Set existing cache value"); + } + + iterator->mNavMeshData = std::move(value); + mUsedNavMeshDataSize += navMeshSize; + mFreeNavMeshDataSize += navMeshSize; + + acquireItemUnsafe(iterator); + + return Value(*this, iterator); + } + + void NavMeshTilesCache::removeLeastRecentlyUsed() + { + const auto& item = mFreeItems.back(); + + const auto agentValues = mValues.find(item.mAgentHalfExtents); + if (agentValues == mValues.end()) + return; + + const auto tileValues = agentValues->second.find(item.mChangedTile); + if (tileValues == agentValues->second.end()) + return; + + const auto value = tileValues->second.find(item.mNavMeshKey); + if (value == tileValues->second.end()) + return; + + mUsedNavMeshDataSize -= static_cast(item.mNavMeshData.mSize); + mFreeItems.pop_back(); + + tileValues->second.erase(value); + if (!tileValues->second.empty()) + return; + + agentValues->second.erase(tileValues); + if (!agentValues->second.empty()) + return; + + mValues.erase(agentValues); + } + + void NavMeshTilesCache::acquireItemUnsafe(ItemIterator iterator) + { + if (++iterator->mUseCount > 1) + return; + + mBusyItems.splice(mBusyItems.end(), mFreeItems, iterator); + mFreeNavMeshDataSize -= static_cast(iterator->mNavMeshData.mSize); + } + + void NavMeshTilesCache::releaseItem(ItemIterator iterator) + { + if (--iterator->mUseCount > 0) + return; + + const std::lock_guard lock(mMutex); + + mFreeItems.splice(mFreeItems.begin(), mBusyItems, iterator); + mFreeNavMeshDataSize += static_cast(iterator->mNavMeshData.mSize); + } +} diff --git a/components/detournavigator/navmeshtilescache.hpp b/components/detournavigator/navmeshtilescache.hpp new file mode 100644 index 0000000000..1d0ecb5dc5 --- /dev/null +++ b/components/detournavigator/navmeshtilescache.hpp @@ -0,0 +1,127 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHTILESCACHE_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHTILESCACHE_H + +#include "offmeshconnection.hpp" +#include "navmeshdata.hpp" +#include "recastmesh.hpp" +#include "tileposition.hpp" + +#include +#include +#include +#include + +namespace DetourNavigator +{ + struct NavMeshDataRef + { + unsigned char* mValue; + int mSize; + }; + + class NavMeshTilesCache + { + public: + struct Item + { + std::atomic mUseCount; + osg::Vec3f mAgentHalfExtents; + TilePosition mChangedTile; + std::string mNavMeshKey; + NavMeshData mNavMeshData; + + Item(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, std::string navMeshKey) + : mUseCount(0) + , mAgentHalfExtents(agentHalfExtents) + , mChangedTile(changedTile) + , mNavMeshKey(std::move(navMeshKey)) + {} + }; + + using ItemIterator = std::list::iterator; + + class Value + { + public: + Value() + : mOwner(nullptr), mIterator() {} + + Value(NavMeshTilesCache& owner, ItemIterator iterator) + : mOwner(&owner), mIterator(iterator) + { + } + + Value(const Value& other) = delete; + + Value(Value&& other) + : mOwner(other.mOwner), mIterator(other.mIterator) + { + other.mIterator = ItemIterator(); + } + + ~Value() + { + if (mIterator != ItemIterator()) + mOwner->releaseItem(mIterator); + } + + Value& operator =(const Value& other) = delete; + + Value& operator =(Value&& other) + { + if (mIterator == other.mIterator) + return *this; + + if (mIterator != ItemIterator()) + mOwner->releaseItem(mIterator); + + mOwner = other.mOwner; + mIterator = other.mIterator; + + other.mIterator = ItemIterator(); + + return *this; + } + + NavMeshDataRef get() const + { + return NavMeshDataRef {mIterator->mNavMeshData.mValue.get(), mIterator->mNavMeshData.mSize}; + } + + operator bool() const + { + return mIterator != ItemIterator(); + } + + private: + NavMeshTilesCache* mOwner; + ItemIterator mIterator; + }; + + NavMeshTilesCache(const std::size_t maxNavMeshDataSize); + + Value get(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, + const RecastMesh& recastMesh, const std::vector& offMeshConnections); + + Value set(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, + const RecastMesh& recastMesh, const std::vector& offMeshConnections, + NavMeshData&& value); + + private: + std::mutex mMutex; + std::size_t mMaxNavMeshDataSize; + std::size_t mUsedNavMeshDataSize; + std::size_t mFreeNavMeshDataSize; + std::list mBusyItems; + std::list mFreeItems; + std::map>> mValues; + + void removeLeastRecentlyUsed(); + + void acquireItemUnsafe(ItemIterator iterator); + + void releaseItem(ItemIterator iterator); + }; +} + +#endif diff --git a/components/detournavigator/objectid.hpp b/components/detournavigator/objectid.hpp new file mode 100644 index 0000000000..3b56924b13 --- /dev/null +++ b/components/detournavigator/objectid.hpp @@ -0,0 +1,50 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_OBJECTID_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_OBJECTID_H + +#include +#include + +namespace DetourNavigator +{ + class ObjectId + { + public: + template + explicit ObjectId(const T value) throw() + : mValue(reinterpret_cast(value)) + { + } + + std::size_t value() const throw() + { + return mValue; + } + + friend bool operator <(const ObjectId lhs, const ObjectId rhs) throw() + { + return lhs.mValue < rhs.mValue; + } + + friend bool operator ==(const ObjectId lhs, const ObjectId rhs) throw() + { + return lhs.mValue == rhs.mValue; + } + + private: + std::size_t mValue; + }; +} + +namespace std +{ + template <> + struct hash + { + std::size_t operator ()(const DetourNavigator::ObjectId value) const throw() + { + return value.value(); + } + }; +} + +#endif diff --git a/components/detournavigator/offmeshconnection.hpp b/components/detournavigator/offmeshconnection.hpp new file mode 100644 index 0000000000..60e8ecbbba --- /dev/null +++ b/components/detournavigator/offmeshconnection.hpp @@ -0,0 +1,15 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_OFFMESHCONNECTION_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_OFFMESHCONNECTION_H + +#include + +namespace DetourNavigator +{ + struct OffMeshConnection + { + osg::Vec3f mStart; + osg::Vec3f mEnd; + }; +} + +#endif diff --git a/components/detournavigator/offmeshconnectionsmanager.hpp b/components/detournavigator/offmeshconnectionsmanager.hpp new file mode 100644 index 0000000000..30d7976aec --- /dev/null +++ b/components/detournavigator/offmeshconnectionsmanager.hpp @@ -0,0 +1,115 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_OFFMESHCONNECTIONSMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_OFFMESHCONNECTIONSMANAGER_H + +#include "settings.hpp" +#include "settingsutils.hpp" +#include "tileposition.hpp" +#include "objectid.hpp" +#include "offmeshconnection.hpp" + +#include + +#include + +#include + +#include +#include +#include +#include +#include + +namespace DetourNavigator +{ + class OffMeshConnectionsManager + { + public: + OffMeshConnectionsManager(const Settings& settings) + : mSettings(settings) + {} + + bool add(const ObjectId id, const OffMeshConnection& value) + { + const auto values = mValues.lock(); + + if (!values->mById.insert(std::make_pair(id, value)).second) + return false; + + const auto startTilePosition = getTilePosition(mSettings, value.mStart); + const auto endTilePosition = getTilePosition(mSettings, value.mEnd); + + values->mByTilePosition[startTilePosition].insert(id); + + if (startTilePosition != endTilePosition) + values->mByTilePosition[endTilePosition].insert(id); + + return true; + } + + boost::optional remove(const ObjectId id) + { + const auto values = mValues.lock(); + + const auto itById = values->mById.find(id); + + if (itById == values->mById.end()) + return boost::none; + + const auto result = itById->second; + + values->mById.erase(itById); + + const auto startTilePosition = getTilePosition(mSettings, result.mStart); + const auto endTilePosition = getTilePosition(mSettings, result.mEnd); + + removeByTilePosition(values->mByTilePosition, startTilePosition, id); + + if (startTilePosition != endTilePosition) + removeByTilePosition(values->mByTilePosition, endTilePosition, id); + + return result; + } + + std::vector get(const TilePosition& tilePosition) + { + std::vector result; + + const auto values = mValues.lock(); + + const auto itByTilePosition = values->mByTilePosition.find(tilePosition); + + if (itByTilePosition == values->mByTilePosition.end()) + return result; + + std::for_each(itByTilePosition->second.begin(), itByTilePosition->second.end(), + [&] (const ObjectId v) + { + const auto itById = values->mById.find(v); + if (itById != values->mById.end()) + result.push_back(itById->second); + }); + + return result; + } + + private: + struct Values + { + std::unordered_map mById; + std::map> mByTilePosition; + }; + + const Settings& mSettings; + Misc::ScopeGuarded mValues; + + void removeByTilePosition(std::map>& valuesByTilePosition, + const TilePosition& tilePosition, const ObjectId id) + { + const auto it = valuesByTilePosition.find(tilePosition); + if (it != valuesByTilePosition.end()) + it->second.erase(id); + } + }; +} + +#endif diff --git a/components/detournavigator/recastmesh.cpp b/components/detournavigator/recastmesh.cpp new file mode 100644 index 0000000000..476799c1f5 --- /dev/null +++ b/components/detournavigator/recastmesh.cpp @@ -0,0 +1,22 @@ +#include "recastmesh.hpp" +#include "exceptions.hpp" + +#include + +namespace DetourNavigator +{ + RecastMesh::RecastMesh(std::vector indices, std::vector vertices, std::vector areaTypes, + std::vector water, const std::size_t trianglesPerChunk) + : mIndices(std::move(indices)) + , mVertices(std::move(vertices)) + , mAreaTypes(std::move(areaTypes)) + , mWater(std::move(water)) + , mChunkyTriMesh(mVertices, mIndices, mAreaTypes, trianglesPerChunk) + { + if (getTrianglesCount() != mAreaTypes.size()) + throw InvalidArgument("Number of flags doesn't match number of triangles: triangles=" + + std::to_string(getTrianglesCount()) + ", areaTypes=" + std::to_string(mAreaTypes.size())); + if (getVerticesCount()) + rcCalcBounds(mVertices.data(), static_cast(getVerticesCount()), mBounds.mMin.ptr(), mBounds.mMax.ptr()); + } +} diff --git a/components/detournavigator/recastmesh.hpp b/components/detournavigator/recastmesh.hpp new file mode 100644 index 0000000000..47d5f79637 --- /dev/null +++ b/components/detournavigator/recastmesh.hpp @@ -0,0 +1,80 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESH_H + +#include "areatype.hpp" +#include "chunkytrimesh.hpp" +#include "bounds.hpp" + +#include +#include +#include + +#include + +#include + +namespace DetourNavigator +{ + class RecastMesh + { + public: + struct Water + { + int mCellSize; + btTransform mTransform; + }; + + RecastMesh(std::vector indices, std::vector vertices, std::vector areaTypes, + std::vector water, const std::size_t trianglesPerChunk); + + const std::vector& getIndices() const + { + return mIndices; + } + + const std::vector& getVertices() const + { + return mVertices; + } + + const std::vector& getAreaTypes() const + { + return mAreaTypes; + } + + const std::vector& getWater() const + { + return mWater; + } + + std::size_t getVerticesCount() const + { + return mVertices.size() / 3; + } + + std::size_t getTrianglesCount() const + { + return mIndices.size() / 3; + } + + const ChunkyTriMesh& getChunkyTriMesh() const + { + return mChunkyTriMesh; + } + + const Bounds& getBounds() const + { + return mBounds; + } + + private: + std::vector mIndices; + std::vector mVertices; + std::vector mAreaTypes; + std::vector mWater; + ChunkyTriMesh mChunkyTriMesh; + Bounds mBounds; + }; +} + +#endif diff --git a/components/detournavigator/recastmeshbuilder.cpp b/components/detournavigator/recastmeshbuilder.cpp new file mode 100644 index 0000000000..e325b7eaf1 --- /dev/null +++ b/components/detournavigator/recastmeshbuilder.cpp @@ -0,0 +1,183 @@ +#include "recastmeshbuilder.hpp" +#include "chunkytrimesh.hpp" +#include "debug.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" +#include "exceptions.hpp" + +#include + +#include +#include +#include +#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, const TileBounds& bounds) + : mSettings(settings) + , mBounds(bounds) + { + mBounds.mMin /= mSettings.get().mRecastScaleFactor; + mBounds.mMax /= mSettings.get().mRecastScaleFactor; + } + + void RecastMeshBuilder::addObject(const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + { + if (shape.isCompound()) + return addObject(static_cast(shape), transform, areaType); + else if (shape.getShapeType() == TERRAIN_SHAPE_PROXYTYPE) + return addObject(static_cast(shape), transform, areaType); + else if (shape.isConcave()) + return addObject(static_cast(shape), transform, areaType); + else if (shape.getShapeType() == BOX_SHAPE_PROXYTYPE) + return addObject(static_cast(shape), transform, areaType); + std::ostringstream message; + message << "Unsupported shape type: " << BroadphaseNativeTypes(shape.getShapeType()); + throw InvalidArgument(message.str()); + } + + void RecastMeshBuilder::addObject(const btCompoundShape& shape, const btTransform& transform, + const AreaType areaType) + { + for (int i = 0, num = shape.getNumChildShapes(); i < num; ++i) + addObject(*shape.getChildShape(i), transform * shape.getChildTransform(i), areaType); + } + + void RecastMeshBuilder::addObject(const btConcaveShape& shape, const btTransform& transform, + const AreaType areaType) + { + return addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* triangle, int, int) + { + for (std::size_t i = 3; i > 0; --i) + addTriangleVertex(transform(triangle[i - 1])); + mAreaTypes.push_back(areaType); + })); + } + + void RecastMeshBuilder::addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform, + const AreaType areaType) + { + return addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* triangle, int, int) + { + for (std::size_t i = 0; i < 3; ++i) + addTriangleVertex(transform(triangle[i])); + mAreaTypes.push_back(areaType); + })); + } + + void RecastMeshBuilder::addObject(const btBoxShape& shape, const btTransform& transform, const AreaType areaType) + { + const auto indexOffset = static_cast(mVertices.size() / 3); + + for (int vertex = 0, count = shape.getNumVertices(); vertex < count; ++vertex) + { + btVector3 position; + shape.getVertex(vertex, position); + addVertex(transform(position)); + } + + const std::array indices {{ + 0, 2, 3, + 3, 1, 0, + 0, 4, 6, + 6, 2, 0, + 0, 1, 5, + 5, 4, 0, + 7, 5, 1, + 1, 3, 7, + 7, 3, 2, + 2, 6, 7, + 7, 6, 4, + 4, 5, 7, + }}; + + std::transform(indices.begin(), indices.end(), std::back_inserter(mIndices), + [&] (int index) { return index + indexOffset; }); + + std::generate_n(std::back_inserter(mAreaTypes), 12, [=] { return areaType; }); + } + + void RecastMeshBuilder::addWater(const int cellSize, const btTransform& transform) + { + mWater.push_back(RecastMesh::Water {cellSize, transform}); + } + + std::shared_ptr RecastMeshBuilder::create() const + { + return std::make_shared(mIndices, mVertices, mAreaTypes, mWater, mSettings.get().mTrianglesPerChunk); + } + + void RecastMeshBuilder::reset() + { + mIndices.clear(); + mVertices.clear(); + mAreaTypes.clear(); + mWater.clear(); + } + + void RecastMeshBuilder::addObject(const btConcaveShape& shape, const btTransform& transform, + btTriangleCallback&& callback) + { + btVector3 aabbMin; + btVector3 aabbMax; + + shape.getAabb(btTransform::getIdentity(), aabbMin, aabbMax); + + aabbMin = transform(aabbMin); + aabbMax = transform(aabbMax); + + aabbMin.setX(std::max(mBounds.mMin.x(), aabbMin.x())); + aabbMin.setX(std::min(mBounds.mMax.x(), aabbMin.x())); + aabbMin.setY(std::max(mBounds.mMin.y(), aabbMin.y())); + aabbMin.setY(std::min(mBounds.mMax.y(), aabbMin.y())); + + aabbMax.setX(std::max(mBounds.mMin.x(), aabbMax.x())); + aabbMax.setX(std::min(mBounds.mMax.x(), aabbMax.x())); + aabbMax.setY(std::max(mBounds.mMin.y(), aabbMax.y())); + aabbMax.setY(std::min(mBounds.mMax.y(), aabbMax.y())); + + const auto inversedTransform = transform.inverse(); + + aabbMin = inversedTransform(aabbMin); + aabbMax = inversedTransform(aabbMax); + + aabbMin.setX(std::min(aabbMin.x(), aabbMax.x())); + aabbMin.setY(std::min(aabbMin.y(), aabbMax.y())); + aabbMin.setZ(std::min(aabbMin.z(), aabbMax.z())); + + aabbMax.setX(std::max(aabbMin.x(), aabbMax.x())); + aabbMax.setY(std::max(aabbMin.y(), aabbMax.y())); + aabbMax.setZ(std::max(aabbMin.z(), aabbMax.z())); + + shape.processAllTriangles(&callback, aabbMin, aabbMax); + } + + void RecastMeshBuilder::addTriangleVertex(const btVector3& worldPosition) + { + mIndices.push_back(static_cast(mVertices.size() / 3)); + addVertex(worldPosition); + } + + void RecastMeshBuilder::addVertex(const btVector3& worldPosition) + { + const auto navMeshPosition = toNavMeshCoordinates(mSettings, makeOsgVec3f(worldPosition)); + 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..2f9d0373db --- /dev/null +++ b/components/detournavigator/recastmeshbuilder.hpp @@ -0,0 +1,57 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHBUILDER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHBUILDER_H + +#include "recastmesh.hpp" +#include "tilebounds.hpp" + +#include + +class btBoxShape; +class btCollisionShape; +class btCompoundShape; +class btConcaveShape; +class btHeightfieldTerrainShape; +class btTriangleCallback; + +namespace DetourNavigator +{ + struct Settings; + + class RecastMeshBuilder + { + public: + RecastMeshBuilder(const Settings& settings, const TileBounds& bounds); + + void addObject(const btCollisionShape& shape, const btTransform& transform, const AreaType areaType); + + void addObject(const btCompoundShape& shape, const btTransform& transform, const AreaType areaType); + + void addObject(const btConcaveShape& shape, const btTransform& transform, const AreaType areaType); + + void addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform, const AreaType areaType); + + void addObject(const btBoxShape& shape, const btTransform& transform, const AreaType areaType); + + void addWater(const int mCellSize, const btTransform& transform); + + std::shared_ptr create() const; + + void reset(); + + private: + std::reference_wrapper mSettings; + TileBounds mBounds; + std::vector mIndices; + std::vector mVertices; + std::vector mAreaTypes; + std::vector mWater; + + void addObject(const btConcaveShape& shape, const btTransform& transform, btTriangleCallback&& callback); + + void addTriangleVertex(const btVector3& worldPosition); + + void addVertex(const btVector3& worldPosition); + }; +} + +#endif diff --git a/components/detournavigator/recastmeshmanager.cpp b/components/detournavigator/recastmeshmanager.cpp new file mode 100644 index 0000000000..1c3a72b59c --- /dev/null +++ b/components/detournavigator/recastmeshmanager.cpp @@ -0,0 +1,97 @@ +#include "recastmeshmanager.hpp" + +#include +#include + +namespace DetourNavigator +{ + RecastMeshManager::RecastMeshManager(const Settings& settings, const TileBounds& bounds) + : mShouldRebuild(false) + , mMeshBuilder(settings, bounds) + { + } + + bool RecastMeshManager::addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + { + const auto iterator = mObjectsOrder.emplace(mObjectsOrder.end(), RecastMeshObject(shape, transform, areaType)); + if (!mObjects.emplace(id, iterator).second) + { + mObjectsOrder.erase(iterator); + return false; + } + mShouldRebuild = true; + return mShouldRebuild; + } + + bool RecastMeshManager::updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType) + { + const auto object = mObjects.find(id); + if (object == mObjects.end()) + return false; + if (!object->second->update(transform, areaType)) + return false; + mShouldRebuild = true; + return mShouldRebuild; + } + + boost::optional RecastMeshManager::removeObject(const ObjectId id) + { + const auto object = mObjects.find(id); + if (object == mObjects.end()) + return boost::none; + const RemovedRecastMeshObject result {object->second->getShape(), object->second->getTransform()}; + mObjectsOrder.erase(object->second); + mObjects.erase(object); + mShouldRebuild = true; + return result; + } + + bool RecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, + const btTransform& transform) + { + const auto iterator = mWaterOrder.emplace(mWaterOrder.end(), Water {cellSize, transform}); + if (!mWater.emplace(cellPosition, iterator).second) + { + mWaterOrder.erase(iterator); + return false; + } + mShouldRebuild = true; + return true; + } + + boost::optional RecastMeshManager::removeWater(const osg::Vec2i& cellPosition) + { + const auto water = mWater.find(cellPosition); + if (water == mWater.end()) + return boost::none; + mShouldRebuild = true; + const auto result = *water->second; + mWaterOrder.erase(water->second); + mWater.erase(water); + return result; + } + + std::shared_ptr RecastMeshManager::getMesh() + { + rebuild(); + return mMeshBuilder.create(); + } + + bool RecastMeshManager::isEmpty() const + { + return mObjects.empty(); + } + + void RecastMeshManager::rebuild() + { + if (!mShouldRebuild) + return; + mMeshBuilder.reset(); + for (const auto& v : mWaterOrder) + mMeshBuilder.addWater(v.mCellSize, v.mTransform); + for (const auto& v : mObjectsOrder) + mMeshBuilder.addObject(v.getShape(), v.getTransform(), v.getAreaType()); + mShouldRebuild = false; + } +} diff --git a/components/detournavigator/recastmeshmanager.hpp b/components/detournavigator/recastmeshmanager.hpp new file mode 100644 index 0000000000..f8602e5145 --- /dev/null +++ b/components/detournavigator/recastmeshmanager.hpp @@ -0,0 +1,66 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHMANAGER_H + +#include "recastmeshbuilder.hpp" +#include "recastmeshobject.hpp" +#include "objectid.hpp" + +#include + +#include + +#include + +#include +#include +#include + +class btCollisionShape; + +namespace DetourNavigator +{ + struct RemovedRecastMeshObject + { + std::reference_wrapper mShape; + btTransform mTransform; + }; + + class RecastMeshManager + { + public: + struct Water + { + int mCellSize; + btTransform mTransform; + }; + + RecastMeshManager(const Settings& settings, const TileBounds& bounds); + + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType); + + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform); + + boost::optional removeWater(const osg::Vec2i& cellPosition); + + boost::optional removeObject(const ObjectId id); + + std::shared_ptr getMesh(); + + bool isEmpty() const; + + private: + bool mShouldRebuild; + RecastMeshBuilder mMeshBuilder; + std::list mObjectsOrder; + std::unordered_map::iterator> mObjects; + std::list mWaterOrder; + std::map::iterator> mWater; + + void rebuild(); + }; +} + +#endif diff --git a/components/detournavigator/recastmeshobject.cpp b/components/detournavigator/recastmeshobject.cpp new file mode 100644 index 0000000000..acaf398c16 --- /dev/null +++ b/components/detournavigator/recastmeshobject.cpp @@ -0,0 +1,68 @@ +#include "recastmeshobject.hpp" + +#include + +#include + +#include +#include + +namespace DetourNavigator +{ + RecastMeshObject::RecastMeshObject(const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + : mShape(shape) + , mTransform(transform) + , mAreaType(areaType) + , mChildren(makeChildrenObjects(shape, mAreaType)) + { + } + + bool RecastMeshObject::update(const btTransform& transform, const AreaType areaType) + { + bool result = false; + if (!(mTransform == transform)) + { + mTransform = transform; + result = true; + } + if (mAreaType != areaType) + { + mAreaType = areaType; + result = true; + } + if (mShape.get().isCompound()) + result = updateCompoundObject(static_cast(mShape.get()), mAreaType, mChildren) + || result; + return result; + } + + bool RecastMeshObject::updateCompoundObject(const btCompoundShape& shape, + const AreaType areaType, std::vector& children) + { + assert(static_cast(shape.getNumChildShapes()) == children.size()); + bool result = false; + for (int i = 0, num = shape.getNumChildShapes(); i < num; ++i) + { + assert(shape.getChildShape(i) == std::addressof(children[static_cast(i)].mShape.get())); + result = children[static_cast(i)].update(shape.getChildTransform(i), areaType) || result; + } + return result; + } + + std::vector makeChildrenObjects(const btCollisionShape& shape, const AreaType areaType) + { + if (shape.isCompound()) + return makeChildrenObjects(static_cast(shape), areaType); + else + return std::vector(); + } + + std::vector makeChildrenObjects(const btCompoundShape& shape, const AreaType areaType) + { + std::vector result; + for (int i = 0, num = shape.getNumChildShapes(); i < num; ++i) + result.emplace_back(*shape.getChildShape(i), shape.getChildTransform(i), areaType); + return result; + } +} diff --git a/components/detournavigator/recastmeshobject.hpp b/components/detournavigator/recastmeshobject.hpp new file mode 100644 index 0000000000..aff4681222 --- /dev/null +++ b/components/detournavigator/recastmeshobject.hpp @@ -0,0 +1,53 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHOBJECT_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHOBJECT_H + +#include "areatype.hpp" + +#include + +#include +#include + +class btCollisionShape; +class btCompoundShape; + +namespace DetourNavigator +{ + class RecastMeshObject + { + public: + RecastMeshObject(const btCollisionShape& shape, const btTransform& transform, const AreaType areaType); + + bool update(const btTransform& transform, const AreaType areaType); + + const btCollisionShape& getShape() const + { + return mShape; + } + + const btTransform& getTransform() const + { + return mTransform; + } + + AreaType getAreaType() const + { + return mAreaType; + } + + private: + std::reference_wrapper mShape; + btTransform mTransform; + AreaType mAreaType; + std::vector mChildren; + + static bool updateCompoundObject(const btCompoundShape& shape, const AreaType areaType, + std::vector& children); + }; + + std::vector makeChildrenObjects(const btCollisionShape& shape, const AreaType areaType); + + std::vector makeChildrenObjects(const btCompoundShape& shape, const AreaType areaType); +} + +#endif diff --git a/components/detournavigator/settings.hpp b/components/detournavigator/settings.hpp new file mode 100644 index 0000000000..3e537c2fb9 --- /dev/null +++ b/components/detournavigator/settings.hpp @@ -0,0 +1,41 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGS_H + +#include + +namespace DetourNavigator +{ + struct Settings + { + bool mEnableWriteRecastMeshToFile; + bool mEnableWriteNavMeshToFile; + bool mEnableRecastMeshFileNameRevision; + bool mEnableNavMeshFileNameRevision; + float mCellHeight; + float mCellSize; + float mDetailSampleDist; + float mDetailSampleMaxError; + float mMaxClimb; + float mMaxSimplificationError; + float mMaxSlope; + float mRecastScaleFactor; + float mSwimHeightScale; + int mBorderSize; + int mMaxEdgeLen; + int mMaxNavMeshQueryNodes; + int mMaxPolys; + int mMaxVertsPerPoly; + int mRegionMergeSize; + int mRegionMinSize; + int mTileSize; + std::size_t mAsyncNavMeshUpdaterThreads; + std::size_t mMaxNavMeshTilesCacheSize; + std::size_t mMaxPolygonPathSize; + std::size_t mMaxSmoothPathSize; + std::size_t mTrianglesPerChunk; + std::string mRecastMeshPathPrefix; + std::string mNavMeshPathPrefix; + }; +} + +#endif diff --git a/components/detournavigator/settingsutils.hpp b/components/detournavigator/settingsutils.hpp new file mode 100644 index 0000000000..d96ea53cc2 --- /dev/null +++ b/components/detournavigator/settingsutils.hpp @@ -0,0 +1,89 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGSUTILS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGSUTILS_H + +#include "settings.hpp" +#include "tilebounds.hpp" +#include "tileposition.hpp" +#include "tilebounds.hpp" + +#include + +#include +#include +#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), + }; + } + + inline float getBorderSize(const Settings& settings) + { + return settings.mBorderSize * settings.mCellSize; + } + + inline float getSwimLevel(const Settings& settings, const float agentHalfExtentsZ) + { + return - settings.mSwimHeightScale * agentHalfExtentsZ; + } + + inline btTransform getSwimLevelTransform(const Settings& settings, const btTransform& transform, + const float agentHalfExtentsZ) + { + return btTransform( + transform.getBasis(), + transform.getOrigin() + btVector3(0, 0, getSwimLevel(settings, agentHalfExtentsZ) - agentHalfExtentsZ) + ); + } +} + +#endif diff --git a/components/detournavigator/sharednavmesh.hpp b/components/detournavigator/sharednavmesh.hpp new file mode 100644 index 0000000000..beb0a3c412 --- /dev/null +++ b/components/detournavigator/sharednavmesh.hpp @@ -0,0 +1,13 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SHAREDNAVMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SHAREDNAVMESH_H + +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + using NavMeshPtr = std::shared_ptr; +} + +#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/tilecachedrecastmeshmanager.cpp b/components/detournavigator/tilecachedrecastmeshmanager.cpp new file mode 100644 index 0000000000..d624800e92 --- /dev/null +++ b/components/detournavigator/tilecachedrecastmeshmanager.cpp @@ -0,0 +1,175 @@ +#include "tilecachedrecastmeshmanager.hpp" +#include "makenavmesh.hpp" +#include "gettilespositions.hpp" +#include "settingsutils.hpp" + +namespace DetourNavigator +{ + TileCachedRecastMeshManager::TileCachedRecastMeshManager(const Settings& settings) + : mSettings(settings) + {} + + bool TileCachedRecastMeshManager::addObject(const ObjectId id, const btCollisionShape& shape, + const btTransform& transform, const AreaType areaType) + { + bool result = false; + auto& tilesPositions = mObjectsTilesPositions[id]; + const auto border = getBorderSize(mSettings); + getTilesPositions(shape, transform, mSettings, [&] (const TilePosition& tilePosition) + { + const auto tiles = mTiles.lock(); + auto tile = tiles->find(tilePosition); + if (tile == tiles->end()) + { + auto tileBounds = makeTileBounds(mSettings, tilePosition); + tileBounds.mMin -= osg::Vec2f(border, border); + tileBounds.mMax += osg::Vec2f(border, border); + tile = tiles->insert(std::make_pair(tilePosition, + CachedRecastMeshManager(mSettings, tileBounds))).first; + } + if (tile->second.addObject(id, shape, transform, areaType)) + { + tilesPositions.push_back(tilePosition); + result = true; + } + }); + if (result) + ++mRevision; + return result; + } + + bool TileCachedRecastMeshManager::updateObject(const ObjectId id, const btTransform& transform, + const AreaType areaType) + { + const auto object = mObjectsTilesPositions.find(id); + if (object == mObjectsTilesPositions.end()) + return false; + bool result = false; + { + const auto tiles = mTiles.lock(); + for (const auto& tilePosition : object->second) + { + const auto tile = tiles->find(tilePosition); + if (tile != tiles->end()) + result = tile->second.updateObject(id, transform, areaType) || result; + } + } + if (result) + ++mRevision; + return result; + } + + boost::optional TileCachedRecastMeshManager::removeObject(const ObjectId id) + { + const auto object = mObjectsTilesPositions.find(id); + if (object == mObjectsTilesPositions.end()) + return boost::none; + boost::optional result; + for (const auto& tilePosition : object->second) + { + const auto tiles = mTiles.lock(); + const auto tile = tiles->find(tilePosition); + if (tile == tiles->end()) + continue; + const auto tileResult = tile->second.removeObject(id); + if (tile->second.isEmpty()) + tiles->erase(tile); + if (tileResult && !result) + result = tileResult; + } + if (result) + ++mRevision; + return result; + } + + bool TileCachedRecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, + const btTransform& transform) + { + const auto border = getBorderSize(mSettings); + + auto& tilesPositions = mWaterTilesPositions[cellPosition]; + + bool result = false; + + if (cellSize == std::numeric_limits::max()) + { + const auto tiles = mTiles.lock(); + for (auto& tile : *tiles) + { + if (tile.second.addWater(cellPosition, cellSize, transform)) + { + tilesPositions.push_back(tile.first); + result = true; + } + } + } + else + { + getTilesPositions(cellSize, transform, mSettings, [&] (const TilePosition& tilePosition) + { + const auto tiles = mTiles.lock(); + auto tile = tiles->find(tilePosition); + if (tile == tiles->end()) + { + auto tileBounds = makeTileBounds(mSettings, tilePosition); + tileBounds.mMin -= osg::Vec2f(border, border); + tileBounds.mMax += osg::Vec2f(border, border); + tile = tiles->insert(std::make_pair(tilePosition, + CachedRecastMeshManager(mSettings, tileBounds))).first; + } + if (tile->second.addWater(cellPosition, cellSize, transform)) + { + tilesPositions.push_back(tilePosition); + result = true; + } + }); + } + + if (result) + ++mRevision; + + return result; + } + + boost::optional TileCachedRecastMeshManager::removeWater(const osg::Vec2i& cellPosition) + { + const auto object = mWaterTilesPositions.find(cellPosition); + if (object == mWaterTilesPositions.end()) + return boost::none; + boost::optional result; + for (const auto& tilePosition : object->second) + { + const auto tiles = mTiles.lock(); + const auto tile = tiles->find(tilePosition); + if (tile == tiles->end()) + continue; + const auto tileResult = tile->second.removeWater(cellPosition); + if (tile->second.isEmpty()) + tiles->erase(tile); + if (tileResult && !result) + result = tileResult; + } + if (result) + ++mRevision; + return result; + } + + std::shared_ptr TileCachedRecastMeshManager::getMesh(const TilePosition& tilePosition) + { + const auto tiles = mTiles.lock(); + const auto it = tiles->find(tilePosition); + if (it == tiles->end()) + return nullptr; + return it->second.getMesh(); + } + + bool TileCachedRecastMeshManager::hasTile(const TilePosition& tilePosition) + { + return mTiles.lockConst()->count(tilePosition); + } + + std::size_t TileCachedRecastMeshManager::getRevision() const + { + return mRevision; + } +} diff --git a/components/detournavigator/tilecachedrecastmeshmanager.hpp b/components/detournavigator/tilecachedrecastmeshmanager.hpp new file mode 100644 index 0000000000..f82ef85c5c --- /dev/null +++ b/components/detournavigator/tilecachedrecastmeshmanager.hpp @@ -0,0 +1,52 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_TILECACHEDRECASTMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_TILECACHEDRECASTMESHMANAGER_H + +#include "cachedrecastmeshmanager.hpp" +#include "tileposition.hpp" + +#include + +#include +#include + +namespace DetourNavigator +{ + class TileCachedRecastMeshManager + { + public: + TileCachedRecastMeshManager(const Settings& settings); + + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType); + + boost::optional removeObject(const ObjectId id); + + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform); + + boost::optional removeWater(const osg::Vec2i& cellPosition); + + std::shared_ptr getMesh(const TilePosition& tilePosition); + + bool hasTile(const TilePosition& tilePosition); + + template + void forEachTilePosition(Function&& function) + { + for (const auto& tile : *mTiles.lock()) + function(tile.first); + } + + std::size_t getRevision() const; + + private: + const Settings& mSettings; + Misc::ScopeGuarded> mTiles; + std::unordered_map> mObjectsTilesPositions; + std::map> mWaterTilesPositions; + std::size_t mRevision = 0; + }; +} + +#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/components/misc/guarded.hpp b/components/misc/guarded.hpp new file mode 100644 index 0000000000..db619569a8 --- /dev/null +++ b/components/misc/guarded.hpp @@ -0,0 +1,109 @@ +#ifndef OPENMW_COMPONENTS_MISC_GUARDED_H +#define OPENMW_COMPONENTS_MISC_GUARDED_H + +#include +#include + +namespace Misc +{ + template + class Locked + { + public: + Locked(std::mutex& mutex, T& value) + : mLock(mutex), mValue(value) + {} + + T& get() const + { + return mValue.get(); + } + + T* operator ->() const + { + return std::addressof(get()); + } + + T& operator *() const + { + return get(); + } + + private: + std::unique_lock mLock; + std::reference_wrapper mValue; + }; + + template + class ScopeGuarded + { + public: + ScopeGuarded() = default; + + ScopeGuarded(const T& value) + : mValue(value) + {} + + ScopeGuarded(T&& value) + : mValue(std::move(value)) + {} + + template + ScopeGuarded(Args&& ... args) + : mValue(std::forward(args) ...) + {} + + ScopeGuarded(const ScopeGuarded& other) + : mMutex() + , mValue(other.lock().get()) + {} + + ScopeGuarded(ScopeGuarded&& other) + : mMutex() + , mValue(std::move(other.lock().get())) + {} + + Locked lock() + { + return Locked(mMutex, mValue); + } + + Locked lockConst() + { + return Locked(mMutex, mValue); + } + + private: + std::mutex mMutex; + T mValue; + }; + + template + class SharedGuarded + { + public: + SharedGuarded() + : mMutex(std::make_shared()) + {} + + SharedGuarded(std::shared_ptr value) + : mMutex(std::make_shared()), mValue(std::move(value)) + {} + + Locked lock() const + { + return Locked(*mMutex, *mValue); + } + + Locked lockConst() const + { + return Locked(*mMutex, *mValue); + } + + private: + std::shared_ptr mMutex; + std::shared_ptr mValue; + }; +} + +#endif diff --git a/components/nifbullet/bulletnifloader.cpp b/components/nifbullet/bulletnifloader.cpp index 20307f2526..1a526c63b6 100644 --- a/components/nifbullet/bulletnifloader.cpp +++ b/components/nifbullet/bulletnifloader.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include @@ -42,27 +41,41 @@ bool pathFileNameStartsWithX(const std::string& path) return letterPos < path.size() && (path[letterPos] == 'x' || path[letterPos] == 'X'); } +void fillTriangleMeshWithTransform(btTriangleMesh& mesh, const Nif::NiTriShapeData& data, const osg::Matrixf &transform) +{ + mesh.preallocateVertices(static_cast(data.vertices.size())); + mesh.preallocateIndices(static_cast(data.triangles.size())); + + const std::vector &vertices = data.vertices; + const std::vector &triangles = data.triangles; + + for (std::size_t i = 0; i < triangles.size(); i += 3) + { + mesh.addTriangle( + getbtVector(vertices[triangles[i + 0]] * transform), + getbtVector(vertices[triangles[i + 1]] * transform), + getbtVector(vertices[triangles[i + 2]] * transform) + ); + } +} + +void fillTriangleMesh(btTriangleMesh& mesh, const Nif::NiTriShapeData& data) +{ + fillTriangleMeshWithTransform(mesh, data, osg::Matrixf()); +} + } namespace NifBullet { -BulletNifLoader::BulletNifLoader() - : mCompoundShape() - , mStaticMesh() -{ -} - -BulletNifLoader::~BulletNifLoader() -{ -} - osg::ref_ptr BulletNifLoader::load(const Nif::File& nif) { mShape = new Resource::BulletShape; - mCompoundShape = nullptr; - mStaticMesh = nullptr; + mCompoundShape.reset(); + mStaticMesh.reset(); + mAvoidStaticMesh.reset(); if (nif.numRoots() < 1) { @@ -111,7 +124,9 @@ osg::ref_ptr BulletNifLoader::load(const Nif::File& nif) { btTransform trans; trans.setIdentity(); - mCompoundShape->addChildShape(trans, new Resource::TriangleMeshShape(mStaticMesh.get(), true)); + std::unique_ptr child(new Resource::TriangleMeshShape(mStaticMesh.get(), true)); + mCompoundShape->addChildShape(trans, child.get()); + child.release(); mStaticMesh.release(); } mShape->mCollisionShape = mCompoundShape.release(); @@ -122,22 +137,26 @@ osg::ref_ptr BulletNifLoader::load(const Nif::File& nif) mStaticMesh.release(); } + if (mAvoidStaticMesh) + { + mShape->mAvoidCollisionShape = new Resource::TriangleMeshShape(mAvoidStaticMesh.get(), false); + mAvoidStaticMesh.release(); + } + return mShape; } } // Find a boundingBox in the node hierarchy. // Return: use bounding box for collision? -bool BulletNifLoader::findBoundingBox(const Nif::Node* node, int flags) +bool BulletNifLoader::findBoundingBox(const Nif::Node* node) { - flags |= node->flags; - if (node->hasBounds) { mShape->mCollisionBoxHalfExtents = node->boundXYZ; mShape->mCollisionBoxTranslate = node->boundPos; - if (flags & Nif::NiNode::Flag_BBoxCollision) + if (node->flags & Nif::NiNode::Flag_BBoxCollision) { return true; } @@ -179,7 +198,7 @@ bool BulletNifLoader::hasAutoGeneratedCollision(const Nif::Node* rootNode) } void BulletNifLoader::handleNode(const std::string& fileName, const Nif::Node *node, int flags, - bool isCollisionNode, bool isAnimated, bool autogenerated) + bool isCollisionNode, bool isAnimated, bool autogenerated, bool avoid) { // Accumulate the flags from all the child nodes. This works for all // the flags we currently use, at least. @@ -192,8 +211,7 @@ void BulletNifLoader::handleNode(const std::string& fileName, const Nif::Node *n isCollisionNode = isCollisionNode || (node->recType == Nif::RC_RootCollisionNode); // Don't collide with AvoidNode shapes - if(node->recType == Nif::RC_AvoidNode) - flags |= 0x800; + avoid = avoid || (node->recType == Nif::RC_AvoidNode); // We encountered a RootCollisionNode inside autogenerated mesh. It is not right. if (node->recType == Nif::RC_RootCollisionNode && autogenerated) @@ -234,7 +252,7 @@ void BulletNifLoader::handleNode(const std::string& fileName, const Nif::Node *n // (occurs in tr_ex_imp_wall_arch_04.nif) if(!node->hasBounds && node->recType == Nif::RC_NiTriShape) { - handleNiTriShape(static_cast(node), flags, getWorldTransform(node), isAnimated); + handleNiTriShape(static_cast(node), flags, getWorldTransform(node), isAnimated, avoid); } } @@ -246,12 +264,13 @@ void BulletNifLoader::handleNode(const std::string& fileName, const Nif::Node *n for(size_t i = 0;i < list.length();i++) { if(!list[i].empty()) - handleNode(fileName, list[i].getPtr(), flags, isCollisionNode, isAnimated, autogenerated); + handleNode(fileName, list[i].getPtr(), flags, isCollisionNode, isAnimated, autogenerated, avoid); } } } -void BulletNifLoader::handleNiTriShape(const Nif::NiTriShape *shape, int flags, const osg::Matrixf &transform, bool isAnimated) +void BulletNifLoader::handleNiTriShape(const Nif::NiTriShape *shape, int flags, const osg::Matrixf &transform, + bool isAnimated, bool avoid) { assert(shape != nullptr); @@ -277,21 +296,7 @@ void BulletNifLoader::handleNiTriShape(const Nif::NiTriShape *shape, int flags, std::unique_ptr childMesh(new btTriangleMesh); - const Nif::NiTriShapeData *data = shape->data.getPtr(); - - childMesh->preallocateVertices(data->vertices.size()); - childMesh->preallocateIndices(data->triangles.size()); - - const std::vector &vertices = data->vertices; - const std::vector &triangles = data->triangles; - - for(size_t i = 0;i < data->triangles.size();i+=3) - { - osg::Vec3f b1 = vertices[triangles[i+0]]; - osg::Vec3f b2 = vertices[triangles[i+1]]; - osg::Vec3f b3 = vertices[triangles[i+2]]; - childMesh->addTriangle(getbtVector(b1), getbtVector(b2), getbtVector(b3)); - } + fillTriangleMesh(*childMesh, shape->data.get()); std::unique_ptr childShape(new Resource::TriangleMeshShape(childMesh.get(), true)); childMesh.release(); @@ -314,27 +319,20 @@ void BulletNifLoader::handleNiTriShape(const Nif::NiTriShape *shape, int flags, mCompoundShape->addChildShape(trans, childShape.get()); childShape.release(); } + else if (avoid) + { + if (!mAvoidStaticMesh) + mAvoidStaticMesh.reset(new btTriangleMesh(false)); + + fillTriangleMeshWithTransform(*mAvoidStaticMesh, shape->data.get(), transform); + } else { if (!mStaticMesh) mStaticMesh.reset(new btTriangleMesh(false)); // Static shape, just transform all vertices into position - const Nif::NiTriShapeData *data = shape->data.getPtr(); - const std::vector &vertices = data->vertices; - const std::vector &triangles = data->triangles; - - mStaticMesh->preallocateVertices(data->vertices.size()); - mStaticMesh->preallocateIndices(data->triangles.size()); - - size_t numtris = data->triangles.size(); - for(size_t i = 0;i < numtris;i+=3) - { - osg::Vec3f b1 = vertices[triangles[i+0]]*transform; - osg::Vec3f b2 = vertices[triangles[i+1]]*transform; - osg::Vec3f b3 = vertices[triangles[i+2]]*transform; - mStaticMesh->addTriangle(getbtVector(b1), getbtVector(b2), getbtVector(b3)); - } + fillTriangleMeshWithTransform(*mStaticMesh, shape->data.get(), transform); } } diff --git a/components/nifbullet/bulletnifloader.hpp b/components/nifbullet/bulletnifloader.hpp index f67ab402ee..de7e6bdcdd 100644 --- a/components/nifbullet/bulletnifloader.hpp +++ b/components/nifbullet/bulletnifloader.hpp @@ -11,6 +11,8 @@ #include #include +#include + #include #include #include @@ -35,10 +37,6 @@ namespace NifBullet class BulletNifLoader { public: - BulletNifLoader(); - - virtual ~BulletNifLoader(); - void warn(const std::string &msg) { Log(Debug::Warning) << "NIFLoader: Warn:" << msg; @@ -53,18 +51,21 @@ public: osg::ref_ptr load(const Nif::File& file); private: - bool findBoundingBox(const Nif::Node* node, int flags = 0); + bool findBoundingBox(const Nif::Node* node); - void handleNode(const std::string& fileName, Nif::Node const *node, int flags, bool isCollisionNode, bool isAnimated=false, bool autogenerated=false); + void handleNode(const std::string& fileName, Nif::Node const *node, int flags, bool isCollisionNode, + bool isAnimated=false, bool autogenerated=false, bool avoid=false); bool hasAutoGeneratedCollision(const Nif::Node *rootNode); - void handleNiTriShape(const Nif::NiTriShape *shape, int flags, const osg::Matrixf& transform, bool isAnimated); + void handleNiTriShape(const Nif::NiTriShape *shape, int flags, const osg::Matrixf& transform, bool isAnimated, bool avoid); std::unique_ptr mCompoundShape; std::unique_ptr mStaticMesh; + std::unique_ptr mAvoidStaticMesh; + osg::ref_ptr mShape; }; diff --git a/components/osghelpers/operators.hpp b/components/osghelpers/operators.hpp new file mode 100644 index 0000000000..fdb0227ead --- /dev/null +++ b/components/osghelpers/operators.hpp @@ -0,0 +1,35 @@ +#ifndef OPENMW_COMPONENTS_OSGHELPERS_OPERATORS_H +#define OPENMW_COMPONENTS_OSGHELPERS_OPERATORS_H + +#include +#include +#include + +#include +#include +#include + +namespace osg +{ + inline std::ostream& operator <<(std::ostream& stream, const Vec2i& value) + { + return stream << "osg::Vec2i(" << value.x() << ", " << value.y() << ")"; + } + + inline std::ostream& operator <<(std::ostream& stream, const Vec2f& value) + { + return stream << "osg::Vec2f(" << std::setprecision(std::numeric_limits::max_exponent10) << value.x() + << ", " << std::setprecision(std::numeric_limits::max_exponent10) << value.y() + << ')'; + } + + inline std::ostream& operator <<(std::ostream& stream, const Vec3f& value) + { + return stream << "osg::Vec3f(" << std::setprecision(std::numeric_limits::max_exponent10) << value.x() + << ", " << std::setprecision(std::numeric_limits::max_exponent10) << value.y() + << ", " << std::setprecision(std::numeric_limits::max_exponent10) << value.z() + << ')'; + } +} + +#endif diff --git a/components/resource/bulletshape.cpp b/components/resource/bulletshape.cpp index 15a747dd3a..e3f6b22b4a 100644 --- a/components/resource/bulletshape.cpp +++ b/components/resource/bulletshape.cpp @@ -13,12 +13,14 @@ namespace Resource BulletShape::BulletShape() : mCollisionShape(nullptr) + , mAvoidCollisionShape(nullptr) { } BulletShape::BulletShape(const BulletShape ©, const osg::CopyOp ©op) : mCollisionShape(duplicateCollisionShape(copy.mCollisionShape)) + , mAvoidCollisionShape(duplicateCollisionShape(copy.mAvoidCollisionShape)) , mCollisionBoxHalfExtents(copy.mCollisionBoxHalfExtents) , mCollisionBoxTranslate(copy.mCollisionBoxTranslate) , mAnimatedShapes(copy.mAnimatedShapes) @@ -27,6 +29,7 @@ BulletShape::BulletShape(const BulletShape ©, const osg::CopyOp ©op) BulletShape::~BulletShape() { + deleteShape(mAvoidCollisionShape); deleteShape(mCollisionShape); } @@ -77,11 +80,23 @@ btCollisionShape* BulletShape::duplicateCollisionShape(const btCollisionShape *s throw std::logic_error(std::string("Unhandled Bullet shape duplication: ")+shape->getName()); } -btCollisionShape *BulletShape::getCollisionShape() +btCollisionShape *BulletShape::getCollisionShape() const { return mCollisionShape; } +btCollisionShape *BulletShape::getAvoidCollisionShape() const +{ + return mAvoidCollisionShape; +} + +void BulletShape::setLocalScaling(const btVector3& scale) +{ + mCollisionShape->setLocalScaling(scale); + if (mAvoidCollisionShape) + mAvoidCollisionShape->setLocalScaling(scale); +} + osg::ref_ptr BulletShape::makeInstance() const { osg::ref_ptr instance (new BulletShapeInstance(this)); @@ -99,6 +114,9 @@ BulletShapeInstance::BulletShapeInstance(osg::ref_ptr source) if (source->mCollisionShape) mCollisionShape = duplicateCollisionShape(source->mCollisionShape); + + if (source->mAvoidCollisionShape) + mAvoidCollisionShape = duplicateCollisionShape(source->mAvoidCollisionShape); } } diff --git a/components/resource/bulletshape.hpp b/components/resource/bulletshape.hpp index b30b5045c2..e77c96327f 100644 --- a/components/resource/bulletshape.hpp +++ b/components/resource/bulletshape.hpp @@ -25,6 +25,7 @@ namespace Resource META_Object(Resource, BulletShape) btCollisionShape* mCollisionShape; + btCollisionShape* mAvoidCollisionShape; // Used for actors. mCollisionShape is used for actors only when we need to autogenerate collision box for creatures. // For now, use one file <-> one resource for simplicity. @@ -41,7 +42,11 @@ namespace Resource btCollisionShape* duplicateCollisionShape(const btCollisionShape* shape) const; - btCollisionShape* getCollisionShape(); + btCollisionShape* getCollisionShape() const; + + btCollisionShape* getAvoidCollisionShape() const; + + void setLocalScaling(const btVector3& scale); private: diff --git a/components/sceneutil/agentpath.cpp b/components/sceneutil/agentpath.cpp new file mode 100644 index 0000000000..aaee4dd1ef --- /dev/null +++ b/components/sceneutil/agentpath.cpp @@ -0,0 +1,71 @@ +#include "agentpath.hpp" +#include "detourdebugdraw.hpp" + +#include +#include + +#include + +namespace +{ + void drawAgent(duDebugDraw& debugDraw, const osg::Vec3f& pos, const float radius, const float height, + const float climb, const unsigned color) + { + debugDraw.depthMask(false); + + duDebugDrawCylinderWire(&debugDraw, pos.x() - radius, pos.z() + 0.02f, pos.y() - radius, pos.x() + radius, + pos.z() + height, pos.y() + radius, color, radius * 0.2f); + + duDebugDrawCircle(&debugDraw, pos.x(), pos.z() + climb, pos.y(), radius, duRGBA(0, 0 , 0, 64), radius * 0.1f); + + const auto colb = duRGBA(0, 0, 0, 196); + debugDraw.begin(DU_DRAW_LINES); + debugDraw.vertex(pos.x(), pos.z() - climb, pos.y(), colb); + debugDraw.vertex(pos.x(), pos.z() + climb, pos.y(), colb); + debugDraw.vertex(pos.x() - radius / 2, pos.z() + 0.02f, pos.y(), colb); + debugDraw.vertex(pos.x() + radius / 2, pos.z() + 0.02f, pos.y(), colb); + debugDraw.vertex(pos.x(), pos.z() + 0.02f, pos.y() - radius / 2, colb); + debugDraw.vertex(pos.x(), pos.z() + 0.02f, pos.y() + radius / 2, colb); + debugDraw.end(); + + debugDraw.depthMask(true); + } +} + +namespace SceneUtil +{ + osg::ref_ptr createAgentPathGroup(const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end, + const DetourNavigator::Settings& settings) + { + using namespace DetourNavigator; + + const osg::ref_ptr group(new osg::Group); + + DebugDraw debugDraw(*group, osg::Vec3f(0, 0, 0), 1); + + const auto agentRadius = halfExtents.x(); + const auto agentHeight = 2.0f * halfExtents.z(); + const auto agentClimb = settings.mMaxClimb; + const auto startColor = duRGBA(128, 25, 0, 192); + const auto endColor = duRGBA(51, 102, 0, 129); + + drawAgent(debugDraw, start, agentRadius, agentHeight, agentClimb, startColor); + drawAgent(debugDraw, end, agentRadius, agentHeight, agentClimb, endColor); + + const auto pathColor = duRGBA(0, 0, 0, 220); + + debugDraw.depthMask(false); + + debugDraw.begin(osg::PrimitiveSet::LINE_STRIP, agentRadius * 0.5f); + debugDraw.vertex(osg::Vec3f(start.x(), start.z() + agentClimb, start.y()).ptr(), startColor); + std::for_each(path.begin(), path.end(), + [&] (const osg::Vec3f& v) { debugDraw.vertex(osg::Vec3f(v.x(), v.z() + agentClimb, v.y()).ptr(), pathColor); }); + debugDraw.vertex(osg::Vec3f(end.x(), end.z() + agentClimb, end.y()).ptr(), endColor); + debugDraw.end(); + + debugDraw.depthMask(true); + + return group; + } +} diff --git a/components/sceneutil/agentpath.hpp b/components/sceneutil/agentpath.hpp new file mode 100644 index 0000000000..a8965d852e --- /dev/null +++ b/components/sceneutil/agentpath.hpp @@ -0,0 +1,26 @@ +#ifndef OPENMW_COMPONENTS_SCENEUTIL_AGENTPATH_H +#define OPENMW_COMPONENTS_SCENEUTIL_AGENTPATH_H + +#include + +#include + +namespace osg +{ + class Group; + class Vec3f; +} + +namespace DetourNavigator +{ + struct Settings; +} + +namespace SceneUtil +{ + osg::ref_ptr createAgentPathGroup(const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end, + const DetourNavigator::Settings& settings); +} + +#endif diff --git a/components/sceneutil/detourdebugdraw.cpp b/components/sceneutil/detourdebugdraw.cpp new file mode 100644 index 0000000000..a0ae67dc29 --- /dev/null +++ b/components/sceneutil/detourdebugdraw.cpp @@ -0,0 +1,128 @@ +#include "detourdebugdraw.hpp" +#include "util.hpp" + +#include + +#include +#include +#include + +#define OPENMW_TO_STRING(X) #X +#define OPENMW_LINE_STRING OPENMW_TO_STRING(__LINE__) + +namespace +{ + using DetourNavigator::operator<<; + + osg::PrimitiveSet::Mode toOsgPrimitiveSetMode(duDebugDrawPrimitives value) + { + switch (value) + { + case DU_DRAW_POINTS: + return osg::PrimitiveSet::POINTS; + case DU_DRAW_LINES: + return osg::PrimitiveSet::LINES; + case DU_DRAW_TRIS: + return osg::PrimitiveSet::TRIANGLES; + case DU_DRAW_QUADS: + return osg::PrimitiveSet::QUADS; + } + throw std::logic_error("Can't convert duDebugDrawPrimitives to osg::PrimitiveSet::Mode, value=" + + std::to_string(value)); + } + +} + +namespace SceneUtil +{ + DebugDraw::DebugDraw(osg::Group& group, const osg::Vec3f& shift, float recastInvertedScaleFactor) + : mGroup(group) + , mShift(shift) + , mRecastInvertedScaleFactor(recastInvertedScaleFactor) + , mDepthMask(false) + , mMode(osg::PrimitiveSet::POINTS) + , mSize(1.0f) + { + } + + void DebugDraw::depthMask(bool state) + { + mDepthMask = state; + } + + void DebugDraw::texture(bool state) + { + if (state) + throw std::logic_error("DebugDraw does not support textures (at " __FILE__ ":" OPENMW_LINE_STRING ")"); + } + + void DebugDraw::begin(osg::PrimitiveSet::Mode mode, float size) + { + mMode = mode; + mVertices = new osg::Vec3Array; + mColors = new osg::Vec4Array; + mSize = size * mRecastInvertedScaleFactor; + } + + void DebugDraw::begin(duDebugDrawPrimitives prim, float size) + { + begin(toOsgPrimitiveSetMode(prim), size); + } + + void DebugDraw::vertex(const float* pos, unsigned color) + { + vertex(pos[0], pos[1], pos[2], color); + } + + void DebugDraw::vertex(const float x, const float y, const float z, unsigned color) + { + addVertex(osg::Vec3f(x, y, z)); + addColor(SceneUtil::colourFromRGBA(color)); + } + + void DebugDraw::vertex(const float* pos, unsigned color, const float* uv) + { + vertex(pos[0], pos[1], pos[2], color, uv[0], uv[1]); + } + + void DebugDraw::vertex(const float, const float, const float, unsigned, const float, const float) + { + throw std::logic_error("Not implemented (at " __FILE__ ":" OPENMW_LINE_STRING ")"); + } + + void DebugDraw::end() + { + osg::ref_ptr stateSet(new osg::StateSet); + stateSet->setMode(GL_BLEND, osg::StateAttribute::ON); + stateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF); + stateSet->setMode(GL_DEPTH, (mDepthMask ? osg::StateAttribute::ON : osg::StateAttribute::OFF)); + stateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); + stateSet->setAttributeAndModes(new osg::LineWidth(mSize)); + stateSet->setAttributeAndModes(new osg::BlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)); + + osg::ref_ptr geometry(new osg::Geometry); + geometry->setStateSet(stateSet); + geometry->setUseDisplayList(false); + geometry->setVertexArray(mVertices); + geometry->setColorArray(mColors, osg::Array::BIND_PER_VERTEX); + geometry->addPrimitiveSet(new osg::DrawArrays(mMode, 0, static_cast(mVertices->size()))); + + mGroup.addChild(geometry); + mColors.release(); + mVertices.release(); + } + + void DebugDraw::addVertex(osg::Vec3f&& position) + { + std::swap(position.y(), position.z()); + mVertices->push_back(position * mRecastInvertedScaleFactor + mShift); + } + + void DebugDraw::addColor(osg::Vec4f&& value) + { + mColors->push_back(value); + } +} + +#undef OPENMW_TO_STRING +#undef OPENMW_LINE_STRING diff --git a/components/sceneutil/detourdebugdraw.hpp b/components/sceneutil/detourdebugdraw.hpp new file mode 100644 index 0000000000..bb170e7ba2 --- /dev/null +++ b/components/sceneutil/detourdebugdraw.hpp @@ -0,0 +1,50 @@ +#include + +#include + +namespace osg +{ + class Group; +} + +namespace SceneUtil +{ + class DebugDraw : public duDebugDraw + { + public: + DebugDraw(osg::Group& group, const osg::Vec3f& shift, float recastInvertedScaleFactor); + + void depthMask(bool state) override; + + void texture(bool state) override; + + void begin(osg::PrimitiveSet::Mode mode, float size); + + void begin(duDebugDrawPrimitives prim, float size) override; + + void vertex(const float* pos, unsigned int color) override; + + void vertex(const float x, const float y, const float z, unsigned int color) override; + + void vertex(const float* pos, unsigned int color, const float* uv) override; + + void vertex(const float x, const float y, const float z, unsigned int color, + const float u, const float v) override; + + void end() override; + + private: + osg::Group& mGroup; + osg::Vec3f mShift; + float mRecastInvertedScaleFactor; + bool mDepthMask; + osg::PrimitiveSet::Mode mMode; + float mSize; + osg::ref_ptr mVertices; + osg::ref_ptr mColors; + + void addVertex(osg::Vec3f&& position); + + void addColor(osg::Vec4f&& value); + }; +} diff --git a/components/sceneutil/navmesh.cpp b/components/sceneutil/navmesh.cpp new file mode 100644 index 0000000000..aeb1779bd6 --- /dev/null +++ b/components/sceneutil/navmesh.cpp @@ -0,0 +1,22 @@ +#include "navmesh.hpp" +#include "detourdebugdraw.hpp" + +#include + +#include + +#include + +namespace SceneUtil +{ + osg::ref_ptr createNavMeshGroup(const dtNavMesh& navMesh, const DetourNavigator::Settings& settings) + { + const osg::ref_ptr group(new osg::Group); + DebugDraw debugDraw(*group, osg::Vec3f(0, 0, 10), 1.0f / settings.mRecastScaleFactor); + dtNavMeshQuery navMeshQuery; + navMeshQuery.init(&navMesh, settings.mMaxNavMeshQueryNodes); + duDebugDrawNavMeshWithClosedList(&debugDraw, navMesh, navMeshQuery, + DU_DRAWNAVMESH_OFFMESHCONS | DU_DRAWNAVMESH_CLOSEDLIST); + return group; + } +} diff --git a/components/sceneutil/navmesh.hpp b/components/sceneutil/navmesh.hpp new file mode 100644 index 0000000000..b255b05756 --- /dev/null +++ b/components/sceneutil/navmesh.hpp @@ -0,0 +1,23 @@ +#ifndef OPENMW_COMPONENTS_SCENEUTIL_NAVMESH_H +#define OPENMW_COMPONENTS_SCENEUTIL_NAVMESH_H + +#include + +class dtNavMesh; + +namespace osg +{ + class Group; +} + +namespace DetourNavigator +{ + struct Settings; +} + +namespace SceneUtil +{ + osg::ref_ptr createNavMeshGroup(const dtNavMesh& navMesh, const DetourNavigator::Settings& settings); +} + +#endif diff --git a/components/sceneutil/util.cpp b/components/sceneutil/util.cpp index eec302965b..bbeda683ae 100644 --- a/components/sceneutil/util.cpp +++ b/components/sceneutil/util.cpp @@ -42,4 +42,15 @@ osg::Vec4f colourFromRGB(unsigned int clr) return colour; } +osg::Vec4f colourFromRGBA(unsigned int value) +{ + return osg::Vec4f(makeOsgColorComponent(value, 0), makeOsgColorComponent(value, 8), + makeOsgColorComponent(value, 16), makeOsgColorComponent(value, 24)); +} + +float makeOsgColorComponent(unsigned int value, unsigned int shift) +{ + return float((value >> shift) & 0xFFu) / 255.0f; +} + } diff --git a/components/sceneutil/util.hpp b/components/sceneutil/util.hpp index 1090997404..156d173d28 100644 --- a/components/sceneutil/util.hpp +++ b/components/sceneutil/util.hpp @@ -15,6 +15,10 @@ namespace SceneUtil osg::Vec4f colourFromRGB (unsigned int clr); + osg::Vec4f colourFromRGBA (unsigned int value); + + float makeOsgColorComponent (unsigned int value, unsigned int shift); + } #endif diff --git a/files/settings-default.cfg b/files/settings-default.cfg index 66c6c65774..a4ccb1c766 100644 --- a/files/settings-default.cfg +++ b/files/settings-default.cfg @@ -533,3 +533,107 @@ 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 size of the non-navigable border around the heightfield. (value >= 0) +border size = 16 + +# 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 + +# Maximum number of polygons per navmesh tile (value = 2^n, 0 < n < 22). Maximum number of navmesh tiles depends on +# this value. 22 bits is a limit to store both tile identifier and polygon identifier (tiles = 2^(22 - log2(polygons))). +# See recastnavigation for more details. +max polygons per tile = 4096 + +# 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 + +# Number of background threads to update nav mesh (value >= 1) +async nav mesh updater threads = 1 + +# Maximum total cached size of all nav mesh tiles in bytes (value >= 0) +max nav mesh tiles cache size = 268435456 + +# 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 + +# Enable debug log (true, false) +enable log = false + +# Write debug log to this file +log path = detournavigator.log + +# Write recast mesh to file in .obj format for each use to update nav mesh (true, false) +enable write recast mesh to file = false + +# Write NavMesh to file to be able to open by RecastDemo (true, false) +enable write nav mesh to file = false + +# Write each recast mesh file with revision in name. Otherwise will rewrite same file. (true, false) +enable recast mesh file name revision = false + +# Write each nav mesh file with revision in name. Otherwise will rewrite same file. (true, false) +enable nav mesh file name revision = false + +# Write recast mesh file at path with this prefix +recast mesh path prefix = + +# Write nav mesh file at path with this prefix +nav mesh path prefix = + +# Render nav mesh (true, false) +enable nav mesh render = false + +# Render agents paths (true, false) +enable agents paths render = false