From b4ec4440573979b222a5c8be5296e8fb80772282 Mon Sep 17 00:00:00 2001
From: elsid <elsid.mail@gmail.com>
Date: Thu, 15 Aug 2019 19:19:04 +0200
Subject: [PATCH] Open door when it is on the way to a next path point

---
 apps/openmw/mwbase/world.hpp          |  2 ++
 apps/openmw/mwmechanics/aipackage.cpp | 15 +++++++++++++++
 apps/openmw/mwmechanics/aipackage.hpp |  2 ++
 apps/openmw/mwworld/worldimp.cpp      | 20 ++++++++++++++++++++
 apps/openmw/mwworld/worldimp.hpp      |  2 ++
 5 files changed, 41 insertions(+)

diff --git a/apps/openmw/mwbase/world.hpp b/apps/openmw/mwbase/world.hpp
index 955e77168d..8c6829eb30 100644
--- a/apps/openmw/mwbase/world.hpp
+++ b/apps/openmw/mwbase/world.hpp
@@ -614,6 +614,8 @@ namespace MWBase
 
             /// Return physical half extents of the given actor to be used in pathfinding
             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;
     };
 }
 
diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp
index ef22d986a4..c834f6984c 100644
--- a/apps/openmw/mwmechanics/aipackage.cpp
+++ b/apps/openmw/mwmechanics/aipackage.cpp
@@ -13,6 +13,8 @@
 #include "../mwworld/cellstore.hpp"
 #include "../mwworld/inventorystore.hpp"
 
+#include "../mwphysics/collisiontype.hpp"
+
 #include "pathgrid.hpp"
 #include "creaturestats.hpp"
 #include "movement.hpp"
@@ -240,6 +242,9 @@ void MWMechanics::AiPackage::openDoors(const MWWorld::Ptr& actor)
 
     if (!door.getCellRef().getTeleport() && door.getClass().getDoorState(door) == MWWorld::DoorState::Idle)
     {
+        if (!isDoorOnTheWay(actor, door))
+            return;
+
         if ((door.getCellRef().getTrap().empty() && door.getCellRef().getLockLevel() <= 0 ))
         {
             world->activate(door, actor);
@@ -408,3 +413,13 @@ DetourNavigator::Flags MWMechanics::AiPackage::getNavigatorFlags(const MWWorld::
 
     return result;
 }
+
+bool MWMechanics::AiPackage::isDoorOnTheWay(const MWWorld::Ptr& actor, const MWWorld::Ptr& door) const
+{
+    const auto world = MWBase::Environment::get().getWorld();
+    const auto halfExtents = world->getHalfExtents(actor);
+    const auto position = actor.getRefData().getPosition().asVec3() + osg::Vec3f(0, 0, halfExtents.z());
+    const auto destination = mPathFinder.getPath().front() + osg::Vec3f(0, 0, halfExtents.z());
+
+    return world->hasCollisionWithDoor(door, position, destination);
+}
diff --git a/apps/openmw/mwmechanics/aipackage.hpp b/apps/openmw/mwmechanics/aipackage.hpp
index 6bb12342a0..0f7b5cf1d0 100644
--- a/apps/openmw/mwmechanics/aipackage.hpp
+++ b/apps/openmw/mwmechanics/aipackage.hpp
@@ -147,6 +147,8 @@ namespace MWMechanics
 
         private:
             bool isNearInactiveCell(osg::Vec3f position);
+
+            bool isDoorOnTheWay(const MWWorld::Ptr& actor, const MWWorld::Ptr& door) const;
     };
 }
 
diff --git a/apps/openmw/mwworld/worldimp.cpp b/apps/openmw/mwworld/worldimp.cpp
index 3ea994683c..908e5c0524 100644
--- a/apps/openmw/mwworld/worldimp.cpp
+++ b/apps/openmw/mwworld/worldimp.cpp
@@ -15,6 +15,7 @@
 #include <components/misc/constants.hpp>
 #include <components/misc/resourcehelpers.hpp>
 #include <components/misc/rng.hpp>
+#include <components/misc/convert.hpp>
 
 #include <components/files/collections.hpp>
 
@@ -3829,4 +3830,23 @@ namespace MWWorld
             return getHalfExtents(actor);
     }
 
+    bool World::hasCollisionWithDoor(const MWWorld::ConstPtr& door, const osg::Vec3f& position, const osg::Vec3f& destination) const
+    {
+        const auto object = mPhysics->getObject(door);
+
+        if (!object)
+            return false;
+
+        btVector3 aabbMin;
+        btVector3 aabbMax;
+        object->getShapeInstance()->getCollisionShape()->getAabb(btTransform::getIdentity(), aabbMin, aabbMax);
+
+        const auto toLocal = object->getCollisionObject()->getWorldTransform().inverse();
+        const auto localFrom = toLocal(Misc::Convert::toBullet(position));
+        const auto localTo = toLocal(Misc::Convert::toBullet(destination));
+
+        btScalar hitDistance = 1;
+        btVector3 hitNormal;
+        return btRayAabb(localFrom, localTo, aabbMin, aabbMax, hitDistance, hitNormal);
+    }
 }
diff --git a/apps/openmw/mwworld/worldimp.hpp b/apps/openmw/mwworld/worldimp.hpp
index 76d0154777..bd34087ac1 100644
--- a/apps/openmw/mwworld/worldimp.hpp
+++ b/apps/openmw/mwworld/worldimp.hpp
@@ -719,6 +719,8 @@ namespace MWWorld
 
             /// Return physical half extents of the given actor to be used in pathfinding
             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;
     };
 }