From e323e6e7e61c75bf15f090ce7993857f8d17a6fc Mon Sep 17 00:00:00 2001 From: elsid Date: Mon, 20 Jan 2020 22:08:25 +0100 Subject: [PATCH 1/2] Consider moved distance in direction to destination for obstacle check Assume actor is stuck when it's not able to move in the destination direction with maximum speed. Approach to check moved distance from the previous point doesn't work good for slow and big actors. When they face obstacle they're trying to move along with oscillation so check is passing but they don't get any closer to the destination. --- apps/openmw/mwmechanics/aipackage.cpp | 3 +- apps/openmw/mwmechanics/obstacle.cpp | 99 ++++++++++++++------------- apps/openmw/mwmechanics/obstacle.hpp | 17 ++--- 3 files changed, 61 insertions(+), 58 deletions(-) diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp index 822d64afac..114e011ce1 100644 --- a/apps/openmw/mwmechanics/aipackage.cpp +++ b/apps/openmw/mwmechanics/aipackage.cpp @@ -197,7 +197,8 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& zTurn(actor, mPathFinder.getZAngleToNext(position.x(), position.y())); smoothTurn(actor, mPathFinder.getXAngleToNext(position.x(), position.y(), position.z()), 0); - mObstacleCheck.update(actor, duration); + const auto destination = mPathFinder.getPath().empty() ? dest : mPathFinder.getPath().front(); + mObstacleCheck.update(actor, destination, duration); // handle obstacles on the way evadeObstacles(actor); diff --git a/apps/openmw/mwmechanics/obstacle.cpp b/apps/openmw/mwmechanics/obstacle.cpp index 2c131ccaef..6268eaddf2 100644 --- a/apps/openmw/mwmechanics/obstacle.cpp +++ b/apps/openmw/mwmechanics/obstacle.cpp @@ -77,89 +77,94 @@ namespace MWMechanics } ObstacleCheck::ObstacleCheck() - : mWalkState(State_Norm) - , mStuckDuration(0) - , mEvadeDuration(0) - , mDistSameSpot(-1) // avoid calculating it each time + : mWalkState(WalkState::Initial) + , mStateDuration(0) , mEvadeDirectionIndex(0) { } void ObstacleCheck::clear() { - mWalkState = State_Norm; - mStuckDuration = 0; - mEvadeDuration = 0; + mWalkState = WalkState::Initial; } bool ObstacleCheck::isEvading() const { - return mWalkState == State_Evade; + return mWalkState == WalkState::Evade; } /* * input - actor, duration (time since last check) * output - true if evasive action needs to be taken * - * Walking state transitions (player greeting check not shown): + * Walking state transitions (player greeting check not shown): * - * MoveNow <------------------------------------+ - * | d| - * | | - * +-> State_Norm <---> State_CheckStuck --> State_Evade - * ^ ^ | f ^ | t ^ | | - * | | | | | | | | - * | +---+ +---+ +---+ | u - * | any < t < u | - * +--------------------------------------------+ + * Initial ----> Norm <--------> CheckStuck -------> Evade ---+ + * ^ ^ | f ^ | t ^ | | + * | | | | | | | | + * | +-+ +---+ +---+ | u + * | any < t < u | + * +---------------------------------------------+ * * f = one reaction time - * d = proximity to a closed door * t = how long before considered stuck * u = how long to move sideways * */ - void ObstacleCheck::update(const MWWorld::Ptr& actor, float duration) + void ObstacleCheck::update(const MWWorld::Ptr& actor, const osg::Vec3f& destination, float duration) { - const osg::Vec3f pos = actor.getRefData().getPosition().asVec3(); + const auto position = actor.getRefData().getPosition().asVec3(); - if (mDistSameSpot == -1) - mDistSameSpot = DIST_SAME_SPOT * actor.getClass().getSpeed(actor); - - const float distSameSpot = mDistSameSpot * duration; - const bool samePosition = (pos - mPrev).length2() < distSameSpot * distSameSpot; - - mPrev = pos; - - if (mWalkState != State_Evade) + if (mWalkState == WalkState::Initial) { - if(!samePosition) + mWalkState = WalkState::Norm; + mStateDuration = 0; + mPrev = position; + return; + } + + if (mWalkState != WalkState::Evade) + { + const float distSameSpot = DIST_SAME_SPOT * actor.getClass().getSpeed(actor) * duration; + const float prevDistance = (destination - mPrev).length(); + const float currentDistance = (destination - position).length(); + const float movedDistance = prevDistance - currentDistance; + + mPrev = position; + + if (movedDistance >= distSameSpot) { - mWalkState = State_Norm; - mStuckDuration = 0; - mEvadeDuration = 0; + mWalkState = WalkState::Norm; + mStateDuration = 0; return; } - mWalkState = State_CheckStuck; - mStuckDuration += duration; - // consider stuck only if position unchanges for a period - if(mStuckDuration < DURATION_SAME_SPOT) - return; // still checking, note duration added to timer - else + if (mWalkState == WalkState::Norm) { - mStuckDuration = 0; - mWalkState = State_Evade; - chooseEvasionDirection(); + mWalkState = WalkState::CheckStuck; + mStateDuration = duration; + return; } + + mStateDuration += duration; + if (mStateDuration < DURATION_SAME_SPOT) + { + return; + } + + mWalkState = WalkState::Evade; + mStateDuration = 0; + chooseEvasionDirection(); + return; } - mEvadeDuration += duration; - if(mEvadeDuration >= DURATION_TO_EVADE) + mStateDuration += duration; + if(mStateDuration >= DURATION_TO_EVADE) { // tried to evade, assume all is ok and start again - mWalkState = State_Norm; - mEvadeDuration = 0; + mWalkState = WalkState::Norm; + mStateDuration = 0; + mPrev = position; } } diff --git a/apps/openmw/mwmechanics/obstacle.hpp b/apps/openmw/mwmechanics/obstacle.hpp index 2934ceb1f1..8314031ea2 100644 --- a/apps/openmw/mwmechanics/obstacle.hpp +++ b/apps/openmw/mwmechanics/obstacle.hpp @@ -32,30 +32,27 @@ namespace MWMechanics bool isEvading() const; // Updates internal state, call each frame for moving actor - void update(const MWWorld::Ptr& actor, float duration); + void update(const MWWorld::Ptr& actor, const osg::Vec3f& destination, float duration); // change direction to try to fix "stuck" actor void takeEvasiveAction(MWMechanics::Movement& actorMovement) const; private: - - // for checking if we're stuck osg::Vec3f mPrev; // directions to try moving in when get stuck static const float evadeDirections[NUM_EVADE_DIRECTIONS][2]; - enum WalkState + enum class WalkState { - State_Norm, - State_CheckStuck, - State_Evade + Initial, + Norm, + CheckStuck, + Evade }; WalkState mWalkState; - float mStuckDuration; // accumulate time here while in same spot - float mEvadeDuration; - float mDistSameSpot; // take account of actor's speed + float mStateDuration; int mEvadeDirectionIndex; void chooseEvasionDirection(); From 1e106013a04aaf1217c6f2449c51aeea5c2a90b7 Mon Sep 17 00:00:00 2001 From: elsid Date: Mon, 20 Jan 2020 23:06:47 +0100 Subject: [PATCH 2/2] Use navmesh to find wander destination outside pathgrid for ground based actors Use dtNavMeshQuery::findRandomPointAroundCircle from recastnavigation --- apps/openmw/mwmechanics/aiwander.cpp | 37 ++++++++++----- .../detournavigator/navigator.cpp | 31 +++++++++++++ components/CMakeLists.txt | 2 + .../findrandompointaroundcircle.cpp | 45 +++++++++++++++++++ .../findrandompointaroundcircle.hpp | 20 +++++++++ components/detournavigator/navigator.cpp | 20 +++++++++ components/detournavigator/navigator.hpp | 12 ++++- 7 files changed, 154 insertions(+), 13 deletions(-) create mode 100644 components/detournavigator/findrandompointaroundcircle.cpp create mode 100644 components/detournavigator/findrandompointaroundcircle.hpp create mode 100644 components/detournavigator/navigator.cpp diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp index 273246261b..b20b1cb971 100644 --- a/apps/openmw/mwmechanics/aiwander.cpp +++ b/apps/openmw/mwmechanics/aiwander.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "../mwbase/world.hpp" #include "../mwbase/environment.hpp" @@ -52,6 +53,14 @@ namespace MWMechanics return 1; return COUNT_BEFORE_RESET; } + + osg::Vec3f getRandomPointAround(const osg::Vec3f& position, const float distance) + { + const float randomDirection = Misc::Rng::rollClosedProbability() * 2.0f * osg::PI; + osg::Matrixf rotation; + rotation.makeRotate(randomDirection, osg::Vec3f(0.0, 0.0, 1.0)); + return position + osg::Vec3f(distance, 0.0, 0.0) * rotation; + } } AiWander::AiWander(int distance, int duration, int timeOfDay, const std::vector& idle, bool repeat): @@ -310,14 +319,24 @@ namespace MWMechanics std::size_t attempts = 10; // If a unit can't wander out of water, don't want to hang here const bool isWaterCreature = actor.getClass().isPureWaterCreature(actor); const bool isFlyingCreature = actor.getClass().isPureFlyingCreature(actor); + const auto world = MWBase::Environment::get().getWorld(); + const auto halfExtents = world->getPathfindingHalfExtents(actor); + const auto navigator = world->getNavigator(); + const auto navigatorFlags = getNavigatorFlags(actor); + do { // Determine a random location within radius of original position const float wanderRadius = (0.2f + Misc::Rng::rollClosedProbability() * 0.8f) * wanderDistance; - const float randomDirection = Misc::Rng::rollClosedProbability() * 2.0f * osg::PI; - const float destinationX = mInitialActorPosition.x() + wanderRadius * std::cos(randomDirection); - const float destinationY = mInitialActorPosition.y() + wanderRadius * std::sin(randomDirection); - const float destinationZ = mInitialActorPosition.z(); - mDestination = osg::Vec3f(destinationX, destinationY, destinationZ); + if (!isWaterCreature && !isFlyingCreature) + { + // findRandomPointAroundCircle uses wanderDistance as limit for random and not as exact distance + if (const auto destination = navigator->findRandomPointAroundCircle(halfExtents, currentPosition, wanderDistance, navigatorFlags)) + mDestination = *destination; + else + mDestination = getRandomPointAround(mInitialActorPosition, wanderRadius); + } + else + mDestination = getRandomPointAround(mInitialActorPosition, wanderRadius); // Check if land creature will walk onto water or if water creature will swim onto land if (!isWaterCreature && destinationIsAtWater(actor, mDestination)) @@ -327,15 +346,9 @@ namespace MWMechanics continue; if (isWaterCreature || isFlyingCreature) - { mPathFinder.buildStraightPath(mDestination); - } else - { - const osg::Vec3f halfExtents = MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(actor); - mPathFinder.buildPathByNavMesh(actor, currentPosition, mDestination, halfExtents, - getNavigatorFlags(actor)); - } + mPathFinder.buildPathByNavMesh(actor, currentPosition, mDestination, halfExtents, navigatorFlags); if (mPathFinder.isPathConstructed()) { diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp index 516f2c60f5..f8aa8f535f 100644 --- a/apps/openmw_test_suite/detournavigator/navigator.cpp +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -2,11 +2,14 @@ #include #include +#include #include #include #include +#include + #include #include @@ -655,4 +658,32 @@ namespace osg::Vec3f(215, -215, 1.87718021869659423828125), })) << mPath; } + + TEST_F(DetourNavigatorNavigatorTest, update_then_find_random_point_around_circle_should_return_position) + { + const std::array heightfieldData {{ + 0, 0, 0, 0, 0, + 0, -25, -25, -25, -25, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + }}; + 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(); + + Misc::Rng::init(42); + + const auto result = mNavigator->findRandomPointAroundCircle(mAgentHalfExtents, mStart, 100.0, Flag_walk); + + ASSERT_EQ(result, boost::optional(osg::Vec3f(-209.95985412597656, 129.89768981933594, -0.26253718137741089))); + + const auto distance = (*result - mStart).length(); + + EXPECT_EQ(distance, 85.260780334472656) << distance; + } } diff --git a/components/CMakeLists.txt b/components/CMakeLists.txt index 33a458b878..45526a5af6 100644 --- a/components/CMakeLists.txt +++ b/components/CMakeLists.txt @@ -172,6 +172,8 @@ add_component_dir(detournavigator recastmeshobject navmeshtilescache settings + navigator + findrandompointaroundcircle ) set (ESM_UI ${CMAKE_SOURCE_DIR}/files/ui/contentselector.ui diff --git a/components/detournavigator/findrandompointaroundcircle.cpp b/components/detournavigator/findrandompointaroundcircle.cpp new file mode 100644 index 0000000000..c894e6681c --- /dev/null +++ b/components/detournavigator/findrandompointaroundcircle.cpp @@ -0,0 +1,45 @@ +#include "findrandompointaroundcircle.hpp" +#include "settings.hpp" +#include "findsmoothpath.hpp" + +#include + +#include +#include +#include + +namespace DetourNavigator +{ + boost::optional findRandomPointAroundCircle(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, + const osg::Vec3f& start, const float maxRadius, const Flags includeFlags, const Settings& settings) + { + 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) + return boost::optional(); + + dtPolyRef resultRef = 0; + osg::Vec3f resultPosition; + navMeshQuery.findRandomPointAroundCircle(startRef, start.ptr(), maxRadius, &queryFilter, + &Misc::Rng::rollProbability, &resultRef, resultPosition.ptr()); + + if (resultRef == 0) + return boost::optional(); + + return boost::optional(resultPosition); + } +} diff --git a/components/detournavigator/findrandompointaroundcircle.hpp b/components/detournavigator/findrandompointaroundcircle.hpp new file mode 100644 index 0000000000..841508f67d --- /dev/null +++ b/components/detournavigator/findrandompointaroundcircle.hpp @@ -0,0 +1,20 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDRANDOMPOINTAROUNDCIRCLE_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDRANDOMPOINTAROUNDCIRCLE_H + +#include "flags.hpp" + +#include + +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + struct Settings; + + boost::optional findRandomPointAroundCircle(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, + const osg::Vec3f& start, const float maxRadius, const Flags includeFlags, const Settings& settings); +} + +#endif diff --git a/components/detournavigator/navigator.cpp b/components/detournavigator/navigator.cpp new file mode 100644 index 0000000000..a58a4f8768 --- /dev/null +++ b/components/detournavigator/navigator.cpp @@ -0,0 +1,20 @@ +#include "findrandompointaroundcircle.hpp" +#include "navigator.hpp" + +namespace DetourNavigator +{ + boost::optional Navigator::findRandomPointAroundCircle(const osg::Vec3f& agentHalfExtents, + const osg::Vec3f& start, const float maxRadius, const Flags includeFlags) const + { + const auto navMesh = getNavMesh(agentHalfExtents); + if (!navMesh) + return boost::optional(); + const auto settings = getSettings(); + const auto result = DetourNavigator::findRandomPointAroundCircle(navMesh->lockConst()->getImpl(), + toNavMeshCoordinates(settings, agentHalfExtents), toNavMeshCoordinates(settings, start), + toNavMeshCoordinates(settings, maxRadius), includeFlags, settings); + if (!result) + return boost::optional(); + return boost::optional(fromNavMeshCoordinates(settings, *result)); + } +} diff --git a/components/detournavigator/navigator.hpp b/components/detournavigator/navigator.hpp index 561b7f02b5..74daab5d7e 100644 --- a/components/detournavigator/navigator.hpp +++ b/components/detournavigator/navigator.hpp @@ -157,7 +157,6 @@ namespace DetourNavigator * @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 float stepSize, const osg::Vec3f& start, @@ -194,6 +193,17 @@ namespace DetourNavigator virtual const Settings& getSettings() const = 0; virtual void reportStats(unsigned int frameNumber, osg::Stats& stats) const = 0; + + /** + * @brief findRandomPointAroundCircle returns random location on navmesh within the reach of specified location. + * @param agentHalfExtents allows to find navmesh for given actor. + * @param start path from given point. + * @param maxRadius limit maximum distance from start. + * @param includeFlags setup allowed surfaces for actor to walk. + * @return not empty optional with position if point is found and empty optional if point is not found. + */ + boost::optional findRandomPointAroundCircle(const osg::Vec3f& agentHalfExtents, + const osg::Vec3f& start, const float maxRadius, const Flags includeFlags) const; }; }