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

@@ -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/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);
+        if (mWalkState == WalkState::Initial)
+        {
+            mWalkState = WalkState::Norm;
+            mStateDuration = 0;
+            mPrev = position;
+            return;
+        }
 
-        const float distSameSpot = mDistSameSpot * duration;
-        const bool samePosition = (pos - mPrev).length2() < distSameSpot * distSameSpot;
+        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 = pos;
+            mPrev = position;
 
-        if (mWalkState != State_Evade)
-        {
-            if(!samePosition)
+            if (movedDistance >= distSameSpot)
+            {
+                mWalkState = WalkState::Norm;
+                mStateDuration = 0;
+                return;
+            }
+
+            if (mWalkState == WalkState::Norm)
             {
-                mWalkState = State_Norm;
-                mStuckDuration = 0;
-                mEvadeDuration = 0;
+                mWalkState = WalkState::CheckStuck;
+                mStateDuration = duration;
                 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
+            mStateDuration += duration;
+            if (mStateDuration < DURATION_SAME_SPOT)
             {
-                mStuckDuration = 0;
-                mWalkState = State_Evade;
-                chooseEvasionDirection();
+                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();