Consider time to destination when try to avoid collision

Actor may reach destination before collision. Get next path point from active
packet pathfinder and use point tolerance to reduce distance.

Use maximum of last destination tolerance and DEFAULT_TOLERANCE because
there are 2 checks made for path completion. First checks whether actor is
close enough to the destination and second drop last path point when actor is
closer than DEFAULT_TOLERANCE.

apps/openmw/mwmechanics/actors.cpp

@@ -1,5 +1,7 @@
 #include "actors.hpp"
 
+#include <optional>
+
 #include <components/esm/esmreader.hpp>
 #include <components/esm/esmwriter.hpp>
 
@@ -173,6 +175,15 @@ namespace MWMechanics
     static const int GREETING_COOLDOWN = 40;    // how many updates should pass before NPC can continue movement
     static const float DECELERATE_DISTANCE = 512.f;
 
+    namespace
+    {
+        float getTimeToDestination(const AiPackage& package, const osg::Vec3f& position, float speed, float duration, const osg::Vec3f& halfExtents)
+        {
+            const auto distanceToNextPathPoint = (package.getNextPathPoint(package.getDestination()) - position).length();
+            return (distanceToNextPathPoint - package.getNextPathPointTolerance(speed, duration, halfExtents)) / speed;
+        }
+    }
+
     class GetStuntedMagickaDuration : public MWMechanics::EffectSourceVisitor
     {
     public:
@@ -1766,7 +1777,7 @@ namespace MWMechanics
 
     }
 
-    void Actors::predictAndAvoidCollisions()
+    void Actors::predictAndAvoidCollisions(float duration)
     {
         if (!MWBase::Environment::get().getMechanicsManager()->isAIActive())
             return;
@@ -1801,7 +1812,8 @@ namespace MWMechanics
             bool shouldAvoidCollision = isMoving;
             bool shouldTurnToApproachingActor = !isMoving;
             MWWorld::Ptr currentTarget; // Combat or pursue target (NPCs should not avoid collision with their targets).
-            for (const auto& package : ptr.getClass().getCreatureStats(ptr).getAiSequence())
+            const auto& aiSequence = ptr.getClass().getCreatureStats(ptr).getAiSequence();
+            for (const auto& package : aiSequence)
             {
                 if (package->getTypeId() == AiPackageTypeId::Follow)
                     shouldAvoidCollision = true;
@@ -1831,6 +1843,10 @@ namespace MWMechanics
             osg::Vec2f movementCorrection(0, 0);
             float angleToApproachingActor = 0;
 
+            const float timeToDestination = aiSequence.isEmpty()
+                    ? std::numeric_limits<float>::max()
+                    : getTimeToDestination(**aiSequence.begin(), basePos, maxSpeed, duration, halfExtents);
+
             // Iterate through all other actors and predict collisions.
             for(PtrActorMap::iterator otherIter(mActors.begin()); otherIter != mActors.end(); ++otherIter)
             {
@@ -1867,7 +1883,7 @@ namespace MWMechanics
                     continue; // No solution; distance is always >= collisionDist.
                 float t = (-vr - std::sqrt(Dh)) / v2;
 
-                if (t < 0 || t > timeToCollision)
+                if (t < 0 || t > timeToCollision || t > timeToDestination)
                     continue;
 
                 // Check visibility and awareness last as it's expensive.
@@ -2077,7 +2093,7 @@ namespace MWMechanics
 
             static const bool avoidCollisions = Settings::Manager::getBool("NPCs avoid collisions", "Game");
             if (avoidCollisions)
-                predictAndAvoidCollisions();
+                predictAndAvoidCollisions(duration);
 
             timerUpdateHeadTrack += duration;
             timerUpdateEquippedLight += duration;

apps/openmw/mwmechanics/actors.hpp

@@ -63,7 +63,7 @@ namespace MWMechanics
 
             void purgeSpellEffects (int casterActorId);
 
-            void predictAndAvoidCollisions();
+            void predictAndAvoidCollisions(float duration);
 
         public:
 

apps/openmw/mwmechanics/aipackage.cpp

@@ -28,6 +28,12 @@ namespace
     {
         return divisor == 0 ? std::numeric_limits<float>::max() * std::numeric_limits<float>::epsilon() : dividend / divisor;
     }
+
+    float getPointTolerance(float speed, float duration, const osg::Vec3f& halfExtents)
+    {
+        const float actorTolerance = 2 * speed * duration + 1.2 * std::max(halfExtents.x(), halfExtents.y());
+        return std::max(MWMechanics::MIN_TOLERANCE, actorTolerance);
+    }
 }
 
 MWMechanics::AiPackage::AiPackage(AiPackageTypeId typeId, const Options& options) :
@@ -98,6 +104,8 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
         return false;
     }
 
+    mLastDestinationTolerance = destTolerance;
+
     const float distToTarget = distance(position, dest);
     const bool isDestReached = (distToTarget <= destTolerance);
     const bool actorCanMoveByZ = canActorMoveByZAxis(actor);
@@ -148,9 +156,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
         }
     }
 
-    const float actorTolerance = 2 * actor.getClass().getMaxSpeed(actor) * duration
-            + 1.2 * std::max(halfExtents.x(), halfExtents.y());
-    const float pointTolerance = std::max(MIN_TOLERANCE, actorTolerance);
+    const float pointTolerance = getPointTolerance(actor.getClass().getMaxSpeed(actor), duration, halfExtents);
 
     static const bool smoothMovement = Settings::Manager::getBool("smooth movement", "Game");
     mPathFinder.update(position, pointTolerance, DEFAULT_TOLERANCE,
@@ -181,7 +187,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
     zTurn(actor, zAngleToNext);
     smoothTurn(actor, mPathFinder.getXAngleToNext(position.x(), position.y(), position.z()), 0);
 
-    const auto destination = mPathFinder.getPath().empty() ? dest : mPathFinder.getPath().front();
+    const auto destination = getNextPathPoint(dest);
     mObstacleCheck.update(actor, destination, duration);
 
     if (smoothMovement)
@@ -461,3 +467,15 @@ DetourNavigator::AreaCosts MWMechanics::AiPackage::getAreaCosts(const MWWorld::P
 
     return costs;
 }
+
+osg::Vec3f MWMechanics::AiPackage::getNextPathPoint(const osg::Vec3f& destination) const
+{
+    return mPathFinder.getPath().empty() ? destination : mPathFinder.getPath().front();
+}
+
+float MWMechanics::AiPackage::getNextPathPointTolerance(float speed, float duration, const osg::Vec3f& halfExtents) const
+{
+    if (mPathFinder.getPathSize() <= 1)
+        return std::max(DEFAULT_TOLERANCE, mLastDestinationTolerance);
+    return getPointTolerance(speed, duration, halfExtents);
+}

apps/openmw/mwmechanics/aipackage.hpp

@@ -122,6 +122,10 @@ namespace MWMechanics
             /// 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 osg::Vec3f& dest);
 
+            osg::Vec3f getNextPathPoint(const osg::Vec3f& destination) const;
+
+            float getNextPathPointTolerance(float speed, float duration, const osg::Vec3f& halfExtents) const;
+
         protected:
             /// Handles path building and shortcutting with obstacles avoiding
             /** \return If the actor has arrived at his destination **/
@@ -166,6 +170,7 @@ namespace MWMechanics
             bool mIsShortcutting;   // if shortcutting at the moment
             bool mShortcutProhibited; // shortcutting may be prohibited after unsuccessful attempt
             osg::Vec3f mShortcutFailPos; // position of last shortcut fail
+            float mLastDestinationTolerance = 0;
 
         private:
             bool isNearInactiveCell(osg::Vec3f position);