diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp index 822d64afa..114e011ce 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/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp index 273246261..b20b1cb97 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/mwmechanics/obstacle.cpp b/apps/openmw/mwmechanics/obstacle.cpp index 2c131ccae..6268eaddf 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 2934ceb1f..8314031ea 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(); diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp index 516f2c60f..f8aa8f535 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 33a458b87..45526a5af 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 000000000..c894e6681 --- /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 000000000..841508f67 --- /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 000000000..a58a4f876 --- /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 561b7f02b..74daab5d7 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; }; }