Check AiTravel destination for other actors presence

Use faster aabbTest but without destance filter. To avoid dependency on a
specific constant and correctly handle situations when there is a big
difference between actors sizes.

CHANGELOG.md

@@ -41,6 +41,7 @@
     Bug #6283: Avis Dorsey follows you after her death
     Bug #6289: Keyword search in dialogues expected the text to be all ASCII characters
     Bug #6302: Teleporting disabled actor breaks its disabled state
+    Bug #6307: Pathfinding in Infidelities quest from Tribunal addon is broken
     Feature #890: OpenMW-CS: Column filtering
     Feature #2554: Modifying an object triggers the instances table to scroll to the corresponding record
     Feature #2780: A way to see current OpenMW version in the console

apps/openmw/mwbase/world.hpp

@@ -653,7 +653,8 @@ namespace MWBase
 
             virtual bool hasCollisionWithDoor(const MWWorld::ConstPtr& door, const osg::Vec3f& position, const osg::Vec3f& destination) const = 0;
 
-            virtual bool isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius, const MWWorld::ConstPtr& ignore) const = 0;
+            virtual bool isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius,
+                const MWWorld::ConstPtr& ignore, std::vector<MWWorld::Ptr>* occupyingActors = nullptr) const = 0;
 
             virtual void reportStats(unsigned int frameNumber, osg::Stats& stats) const = 0;
 

apps/openmw/mwmechanics/aitravel.cpp

@@ -1,5 +1,7 @@
 #include "aitravel.hpp"
 
+#include <algorithm>
+
 #include <components/esm/aisequence.hpp>
 
 #include "../mwbase/environment.hpp"
@@ -23,6 +25,11 @@ bool isWithinMaxRange(const osg::Vec3f& pos1, const osg::Vec3f& pos2)
     return (pos1 - pos2).length2() <= 7168*7168;
 }
 
+    float getActorRadius(const MWWorld::ConstPtr& actor)
+    {
+        const osg::Vec3f halfExtents = MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(actor);
+        return std::max(halfExtents.x(), std::max(halfExtents.y(), halfExtents.z()));
+    }
 }
 
 namespace MWMechanics
@@ -70,18 +77,26 @@ namespace MWMechanics
 
         // Unfortunately, with vanilla assets destination is sometimes blocked by other actor.
         // If we got close to target, check for actors nearby. If they are, finish AI package.
-        int destinationTolerance = 64;
-        if (distance(actorPos, targetPos) <= destinationTolerance)
+        if (mDestinationCheck.update(duration) == Misc::TimerStatus::Elapsed)
         {
-            std::vector<MWWorld::Ptr> targetActors;
-            std::pair<MWWorld::Ptr, osg::Vec3f> result = MWBase::Environment::get().getWorld()->getHitContact(actor, destinationTolerance, targetActors);
-
-            if (!result.first.isEmpty())
+            std::vector<MWWorld::Ptr> occupyingActors;
+            if (isAreaOccupiedByOtherActor(actor, targetPos, &occupyingActors))
+            {
+                const float actorRadius = getActorRadius(actor);
+                const float distanceToTarget = distance(actorPos, targetPos);
+                for (const MWWorld::Ptr& other : occupyingActors)
+                {
+                    const float otherRadius = getActorRadius(other);
+                    const auto [minRadius, maxRadius] = std::minmax(actorRadius, otherRadius);
+                    constexpr float toleranceFactor = 1.25;
+                    if (minRadius * toleranceFactor + maxRadius > distanceToTarget)
                     {
                         actor.getClass().getMovementSettings(actor).mPosition[1] = 0;
                         return true;
                     }
                 }
+            }
+        }
 
         if (pathTo(actor, targetPos, duration))
         {

apps/openmw/mwmechanics/aitravel.hpp

@@ -52,6 +52,8 @@ namespace MWMechanics
             const float mZ;
 
             const bool mHidden;
+
+            AiReactionTimer mDestinationCheck;
     };
 
     struct AiInternalTravel final : public AiTravel

apps/openmw/mwmechanics/aiwander.cpp

@@ -85,14 +85,6 @@ namespace MWMechanics
             return MWBase::Environment::get().getWorld()->castRay(position, visibleDestination, mask, actor);
         }
 
-        bool isAreaOccupiedByOtherActor(const MWWorld::ConstPtr &actor, const osg::Vec3f& destination)
-        {
-            const auto world = MWBase::Environment::get().getWorld();
-            const osg::Vec3f halfExtents = world->getPathfindingHalfExtents(actor);
-            const auto maxHalfExtent = std::max(halfExtents.x(), std::max(halfExtents.y(), halfExtents.z()));
-            return world->isAreaOccupiedByOtherActor(destination, 2 * maxHalfExtent, actor);
-        }
-
         void stopMovement(const MWWorld::Ptr& actor)
         {
             actor.getClass().getMovementSettings(actor).mPosition[0] = 0;

apps/openmw/mwmechanics/obstacle.cpp

@@ -4,6 +4,8 @@
 
 #include "../mwworld/class.hpp"
 #include "../mwworld/cellstore.hpp"
+#include "../mwbase/environment.hpp"
+#include "../mwbase/world.hpp"
 
 #include "movement.hpp"
 
@@ -72,6 +74,15 @@ namespace MWMechanics
         return MWWorld::Ptr(); // none found
     }
 
+    bool isAreaOccupiedByOtherActor(const MWWorld::ConstPtr& actor, const osg::Vec3f& destination,
+        std::vector<MWWorld::Ptr>* occupyingActors)
+    {
+        const auto world = MWBase::Environment::get().getWorld();
+        const osg::Vec3f halfExtents = world->getPathfindingHalfExtents(actor);
+        const auto maxHalfExtent = std::max(halfExtents.x(), std::max(halfExtents.y(), halfExtents.z()));
+        return world->isAreaOccupiedByOtherActor(destination, 2 * maxHalfExtent, actor, occupyingActors);
+    }
+
     ObstacleCheck::ObstacleCheck()
       : mWalkState(WalkState::Initial)
       , mStateDuration(0)

apps/openmw/mwmechanics/obstacle.hpp

@@ -3,9 +3,12 @@
 
 #include <osg/Vec3f>
 
+#include <vector>
+
 namespace MWWorld
 {
     class Ptr;
+    class ConstPtr;
 }
 
 namespace MWMechanics
@@ -21,6 +24,9 @@ namespace MWMechanics
     /** \return Pointer to the door, or empty pointer if none exists **/
     const MWWorld::Ptr getNearbyDoor(const MWWorld::Ptr& actor, float minDist);
 
+    bool isAreaOccupiedByOtherActor(const MWWorld::ConstPtr& actor, const osg::Vec3f& destination,
+        std::vector<MWWorld::Ptr>* occupyingActors = nullptr);
+
     class ObstacleCheck
     {
         public:

apps/openmw/mwphysics/hasspherecollisioncallback.hpp

@@ -22,28 +22,36 @@ namespace MWPhysics
         return nearest.distance(position) < radius;
     }
 
+    template <class OnCollision>
     class HasSphereCollisionCallback final : public btBroadphaseAabbCallback
     {
     public:
         HasSphereCollisionCallback(const btVector3& position, const btScalar radius, btCollisionObject* object,
-                const int mask, const int group)
+                const int mask, const int group, OnCollision* onCollision)
             : mPosition(position),
               mRadius(radius),
               mCollisionObject(object),
               mCollisionFilterMask(mask),
-              mCollisionFilterGroup(group)
+              mCollisionFilterGroup(group),
+              mOnCollision(onCollision)
         {
         }
 
         bool process(const btBroadphaseProxy* proxy) override
         {
-            if (mResult)
+            if (mResult && mOnCollision == nullptr)
                 return false;
             const auto collisionObject = static_cast<btCollisionObject*>(proxy->m_clientObject);
-            if (collisionObject == mCollisionObject)
+            if (collisionObject == mCollisionObject
+                || !needsCollision(*proxy)
+                || !testAabbAgainstSphere(proxy->m_aabbMin, proxy->m_aabbMax, mPosition, mRadius))
                 return true;
-            if (needsCollision(*proxy))
-                mResult = testAabbAgainstSphere(proxy->m_aabbMin, proxy->m_aabbMax, mPosition, mRadius);
+            mResult = true;
+            if (mOnCollision != nullptr)
+            {
+                (*mOnCollision)(collisionObject);
+                return true;
+            }
             return !mResult;
         }
 
@@ -58,6 +66,7 @@ namespace MWPhysics
         btCollisionObject* mCollisionObject;
         int mCollisionFilterMask;
         int mCollisionFilterGroup;
+        OnCollision* mOnCollision;
         bool mResult = false;
 
         bool needsCollision(const btBroadphaseProxy& proxy) const

apps/openmw/mwphysics/physicssystem.cpp

@@ -923,7 +923,8 @@ namespace MWPhysics
                                                     CollisionType_Actor|CollisionType_Projectile);
     }
 
-    bool PhysicsSystem::isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius, const MWWorld::ConstPtr& ignore) const
+    bool PhysicsSystem::isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius,
+        const MWWorld::ConstPtr& ignore, std::vector<MWWorld::Ptr>* occupyingActors) const
     {
         btCollisionObject* object = nullptr;
         const auto it = mActors.find(ignore.mRef);
@@ -934,7 +935,19 @@ namespace MWPhysics
         const auto aabbMax = bulletPosition + btVector3(radius, radius, radius);
         const int mask = MWPhysics::CollisionType_Actor;
         const int group = 0xff;
-        HasSphereCollisionCallback callback(bulletPosition, radius, object, mask, group);
+        if (occupyingActors == nullptr)
+        {
+            HasSphereCollisionCallback callback(bulletPosition, radius, object, mask, group,
+                                                static_cast<void (*)(const btCollisionObject*)>(nullptr));
+            mTaskScheduler->aabbTest(aabbMin, aabbMax, callback);
+            return callback.getResult();
+        }
+        const auto onCollision = [&] (const btCollisionObject* object)
+        {
+            if (PtrHolder* holder = static_cast<PtrHolder*>(object->getUserPointer()))
+                occupyingActors->push_back(holder->getPtr());
+        };
+        HasSphereCollisionCallback callback(bulletPosition, radius, object, mask, group, &onCollision);
         mTaskScheduler->aabbTest(aabbMin, aabbMax, callback);
         return callback.getResult();
     }

apps/openmw/mwphysics/physicssystem.hpp

@@ -252,7 +252,8 @@ namespace MWPhysics
                 std::for_each(mAnimatedObjects.begin(), mAnimatedObjects.end(), function);
             }
 
-            bool isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius, const MWWorld::ConstPtr& ignore) const;
+            bool isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius,
+                const MWWorld::ConstPtr& ignore, std::vector<MWWorld::Ptr>* occupyingActors) const;
 
             void reportStats(unsigned int frameNumber, osg::Stats& stats) const;
             void reportCollision(const btVector3& position, const btVector3& normal);

apps/openmw/mwworld/worldimp.cpp

@@ -3935,9 +3935,10 @@ namespace MWWorld
         return btRayAabb(localFrom, localTo, aabbMin, aabbMax, hitDistance, hitNormal);
     }
 
-    bool World::isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius, const MWWorld::ConstPtr& ignore) const
+    bool World::isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius,
+        const MWWorld::ConstPtr& ignore, std::vector<MWWorld::Ptr>* occupyingActors) const
     {
-        return mPhysics->isAreaOccupiedByOtherActor(position, radius, ignore);
+        return mPhysics->isAreaOccupiedByOtherActor(position, radius, ignore, occupyingActors);
     }
 
     void World::reportStats(unsigned int frameNumber, osg::Stats& stats) const

apps/openmw/mwworld/worldimp.hpp

@@ -735,7 +735,8 @@ namespace MWWorld
 
             bool hasCollisionWithDoor(const MWWorld::ConstPtr& door, const osg::Vec3f& position, const osg::Vec3f& destination) const override;
 
-            bool isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius, const MWWorld::ConstPtr& ignore) const override;
+            bool isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius,
+                const MWWorld::ConstPtr& ignore, std::vector<MWWorld::Ptr>* occupyingActors) const override;
 
             void reportStats(unsigned int frameNumber, osg::Stats& stats) const override;