Merge pull request #2670 from elsid/fix_aiwander_stuck

Fix AiWander stuck (bug #5237)

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);

apps/openmw/mwmechanics/aiwander.cpp

@@ -3,6 +3,7 @@
 #include <components/debug/debuglog.hpp>
 #include <components/misc/rng.hpp>
 #include <components/esm/aisequence.hpp>
+#include <components/detournavigator/navigator.hpp>
 
 #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<unsigned char>& 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())
             {

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;
         }
     }
 

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();

apps/openmw_test_suite/detournavigator/navigator.cpp

@@ -2,11 +2,14 @@
 
 #include <components/detournavigator/navigatorimpl.hpp>
 #include <components/detournavigator/exceptions.hpp>
+#include <components/misc/rng.hpp>
 
 #include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
 #include <BulletCollision/CollisionShapes/btBoxShape.h>
 #include <BulletCollision/CollisionShapes/btCompoundShape.h>
 
+#include <boost/optional/optional_io.hpp>
+
 #include <gtest/gtest.h>
 
 #include <iterator>
@@ -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<btScalar, 5 * 5> 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>(osg::Vec3f(-209.95985412597656, 129.89768981933594, -0.26253718137741089)));
+
+        const auto distance = (*result - mStart).length();
+
+        EXPECT_EQ(distance, 85.260780334472656) << distance;
+    }
 }

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

components/detournavigator/findrandompointaroundcircle.cpp

@@ -0,0 +1,45 @@
+#include "findrandompointaroundcircle.hpp"
+#include "settings.hpp"
+#include "findsmoothpath.hpp"
+
+#include <components/misc/rng.hpp>
+
+#include <DetourCommon.h>
+#include <DetourNavMesh.h>
+#include <DetourNavMeshQuery.h>
+
+namespace DetourNavigator
+{
+    boost::optional<osg::Vec3f> 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<osg::Vec3f>();
+
+        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<osg::Vec3f>();
+
+        return boost::optional<osg::Vec3f>(resultPosition);
+    }
+}

components/detournavigator/findrandompointaroundcircle.hpp

@@ -0,0 +1,20 @@
+#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDRANDOMPOINTAROUNDCIRCLE_H
+#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDRANDOMPOINTAROUNDCIRCLE_H
+
+#include "flags.hpp"
+
+#include <boost/optional.hpp>
+
+#include <osg/Vec3f>
+
+class dtNavMesh;
+
+namespace DetourNavigator
+{
+    struct Settings;
+
+    boost::optional<osg::Vec3f> findRandomPointAroundCircle(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents,
+        const osg::Vec3f& start, const float maxRadius, const Flags includeFlags, const Settings& settings);
+}
+
+#endif

components/detournavigator/navigator.cpp

@@ -0,0 +1,20 @@
+#include "findrandompointaroundcircle.hpp"
+#include "navigator.hpp"
+
+namespace DetourNavigator
+{
+    boost::optional<osg::Vec3f> 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<osg::Vec3f>();
+        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<osg::Vec3f>();
+        return boost::optional<osg::Vec3f>(fromNavMeshCoordinates(settings, *result));
+    }
+}

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 <class OutputIterator>
         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<osg::Vec3f> findRandomPointAroundCircle(const osg::Vec3f& agentHalfExtents,
+            const osg::Vec3f& start, const float maxRadius, const Flags includeFlags) const;
     };
 }