Do not wander to occupied area by other actor

apps/openmw/mwbase/world.hpp

@@ -625,6 +625,8 @@ namespace MWBase
             virtual osg::Vec3f getPathfindingHalfExtents(const MWWorld::ConstPtr& actor) const = 0;
 
             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;
     };
 }
 

apps/openmw/mwmechanics/aiwander.cpp

@@ -81,6 +81,14 @@ namespace MWMechanics
                 | MWPhysics::CollisionType_Actor;
             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);
+        }
     }
 
     AiWander::AiWander(int distance, int duration, int timeOfDay, const std::vector<unsigned char>& idle, bool repeat):
@@ -351,6 +359,9 @@ namespace MWMechanics
             if (isDestinationHidden(actor, mDestination))
                 continue;
 
+            if (isAreaOccupiedByOtherActor(actor, mDestination))
+                continue;
+
             if (isWaterCreature || isFlyingCreature)
                 mPathFinder.buildStraightPath(mDestination);
             else

apps/openmw/mwphysics/hasspherecollisioncallback.hpp

@@ -0,0 +1,72 @@
+#ifndef OPENMW_MWPHYSICS_HASSPHERECOLLISIONCALLBACK_H
+#define OPENMW_MWPHYSICS_HASSPHERECOLLISIONCALLBACK_H
+
+#include <LinearMath/btVector3.h>
+#include <BulletCollision/BroadphaseCollision/btBroadphaseInterface.h>
+#include <BulletCollision/CollisionDispatch/btCollisionObject.h>
+#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
+
+#include <algorithm>
+
+namespace MWPhysics
+{
+    // https://developer.mozilla.org/en-US/docs/Games/Techniques/3D_collision_detection
+    bool testAabbAgainstSphere(const btVector3& aabbMin, const btVector3& aabbMax,
+        const btVector3& position, const btScalar radius)
+    {
+        const btVector3 nearest(
+            std::max(aabbMin.x(), std::min(aabbMax.x(), position.x())),
+            std::max(aabbMin.y(), std::min(aabbMax.y(), position.y())),
+            std::max(aabbMin.z(), std::min(aabbMax.z(), position.z()))
+        );
+        return nearest.distance(position) < radius;
+    }
+
+    class HasSphereCollisionCallback final : public btBroadphaseAabbCallback
+    {
+    public:
+        HasSphereCollisionCallback(const btVector3& position, const btScalar radius, btCollisionObject* object,
+                const int mask, const int group)
+            : mPosition(position),
+              mRadius(radius),
+              mCollisionObject(object),
+              mCollisionFilterMask(mask),
+              mCollisionFilterGroup(group)
+        {
+        }
+
+        bool process(const btBroadphaseProxy* proxy) final
+        {
+            if (mResult)
+                return false;
+            const auto collisionObject = static_cast<btCollisionObject*>(proxy->m_clientObject);
+            if (collisionObject == mCollisionObject)
+                return true;
+            if (needsCollision(*proxy))
+                mResult = testAabbAgainstSphere(proxy->m_aabbMin, proxy->m_aabbMax, mPosition, mRadius);
+            return !mResult;
+        }
+
+        bool getResult() const
+        {
+            return mResult;
+        }
+
+    private:
+        btVector3 mPosition;
+        btScalar mRadius;
+        btCollisionObject* mCollisionObject;
+        int mCollisionFilterMask;
+        int mCollisionFilterGroup;
+        bool mResult = false;
+
+        bool needsCollision(const btBroadphaseProxy& proxy) const
+        {
+            bool collides = (proxy.m_collisionFilterGroup & mCollisionFilterMask) != 0;
+            collides = collides && (mCollisionFilterGroup & proxy.m_collisionFilterMask);
+            return collides;
+        }
+    };
+}
+
+#endif

apps/openmw/mwphysics/physicssystem.cpp

@@ -45,6 +45,7 @@
 #include "trace.h"
 #include "object.hpp"
 #include "heightfield.hpp"
+#include "hasspherecollisioncallback.hpp"
 
 namespace MWPhysics
 {
@@ -1444,4 +1445,20 @@ namespace MWPhysics
         mCollisionWorld->addCollisionObject(mWaterCollisionObject.get(), CollisionType_Water,
                                                     CollisionType_Actor);
     }
+
+    bool PhysicsSystem::isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius, const MWWorld::ConstPtr& ignore) const
+    {
+        btCollisionObject* object = nullptr;
+        const auto it = mActors.find(ignore);
+        if (it != mActors.end())
+            object = it->second->getCollisionObject();
+        const auto bulletPosition = Misc::Convert::toBullet(position);
+        const auto aabbMin = bulletPosition - btVector3(radius, radius, radius);
+        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);
+        mCollisionWorld->getBroadphase()->aabbTest(aabbMin, aabbMax, callback);
+        return callback.getResult();
+    }
 }

apps/openmw/mwphysics/physicssystem.hpp

@@ -187,6 +187,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;
+
         private:
 
             void updateWater();

apps/openmw/mwworld/worldimp.cpp

@@ -3914,4 +3914,9 @@ namespace MWWorld
         btVector3 hitNormal;
         return btRayAabb(localFrom, localTo, aabbMin, aabbMax, hitDistance, hitNormal);
     }
+
+    bool World::isAreaOccupiedByOtherActor(const osg::Vec3f& position, const float radius, const MWWorld::ConstPtr& ignore) const
+    {
+        return mPhysics->isAreaOccupiedByOtherActor(position, radius, ignore);
+    }
 }

apps/openmw/mwworld/worldimp.hpp

@@ -728,6 +728,8 @@ namespace MWWorld
             osg::Vec3f getPathfindingHalfExtents(const MWWorld::ConstPtr& actor) const override;
 
             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;
     };
 }