From 72786fef9d935df0d2b28dd74d1beafc8f134d7f Mon Sep 17 00:00:00 2001
From: mrcheko <cheko@sevas.ua>
Date: Fri, 9 Sep 2016 23:57:19 +0300
Subject: [PATCH] prevent running in circles around path points

addresses http://bugs.openmw.org/issues/2229
---
 apps/openmw/mwmechanics/aipackage.cpp | 43 +++++++++++++++++++++++++--
 apps/openmw/mwmechanics/aipackage.hpp |  5 ++++
 2 files changed, 46 insertions(+), 2 deletions(-)

diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp
index 9f6ef2597..fa952c414 100644
--- a/apps/openmw/mwmechanics/aipackage.cpp
+++ b/apps/openmw/mwmechanics/aipackage.cpp
@@ -19,12 +19,15 @@
 #include "actorutil.hpp"
 #include "coordinateconverter.hpp"
 
+#include <osg/Quat>
+
 MWMechanics::AiPackage::~AiPackage() {}
 
 MWMechanics::AiPackage::AiPackage() : 
     mTimer(AI_REACTION_TIME + 1.0f), // to force initial pathbuild
     mIsShortcutting(false),
-    mShortcutProhibited(false), mShortcutFailPos()
+    mShortcutProhibited(false), mShortcutFailPos(),
+    mRotateOnTheRunChecks(0)
 {
 }
 
@@ -104,6 +107,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgr
             if (wasShortcutting || doesPathNeedRecalc(dest, actor.getCell())) // if need to rebuild path
             {
                 mPathFinder.buildSyncedPath(start, dest, actor.getCell());
+                mRotateOnTheRunChecks = 3;
 
                 // give priority to go directly on target if there is minimal opportunity
                 if (destInLOS && mPathFinder.getPath().size() > 1)
@@ -143,7 +147,13 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgr
     }
     else
     {
-        actor.getClass().getMovementSettings(actor).mPosition[1] = 1; // run to target
+        if (mRotateOnTheRunChecks == 0
+            || isReachableRotatingOnTheRun(actor, *mPathFinder.getPath().begin())) // to prevent circling around a path point
+        {
+            actor.getClass().getMovementSettings(actor).mPosition[1] = 1; // move to the target
+            if (mRotateOnTheRunChecks > 0) mRotateOnTheRunChecks--;
+        }
+
         // handle obstacles on the way
         evadeObstacles(actor, duration, pos);
     }
@@ -292,3 +302,32 @@ bool MWMechanics::AiPackage::isNearInactiveCell(const ESM::Position& actorPos)
         return false;
     }
 }
+
+bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest)
+{
+    // get actor's shortest radius for moving in circle
+    float speed = actor.getClass().getSpeed(actor);
+    speed += speed * 0.1f; // 10% real speed inaccuracy
+    float radius = speed / MAX_VEL_ANGULAR_RADIANS;
+
+    // get radius direction to the center
+    const float* rot = actor.getRefData().getPosition().rot;
+    osg::Quat quatRot(rot[0], -osg::X_AXIS, rot[1], -osg::Y_AXIS, rot[2], -osg::Z_AXIS);
+    osg::Vec3f dir = quatRot * osg::Y_AXIS; // actor's orientation direction is a tangent to circle
+    osg::Vec3f radiusDir = dir ^ osg::Z_AXIS; // radius is perpendicular to a tangent
+    radiusDir.normalize();
+    radiusDir *= radius;
+    
+    // pick up the nearest center candidate
+    osg::Vec3f dest_ = PathFinder::MakeOsgVec3(dest);
+    osg::Vec3f pos = actor.getRefData().getPosition().asVec3();
+    osg::Vec3f center1 = pos - radiusDir;
+    osg::Vec3f center2 = pos + radiusDir;
+    osg::Vec3f center = (center1 - dest_).length2() < (center2 - dest_).length2() ? center1 : center2;
+
+    float distToDest = (center - dest_).length();
+
+    // if pathpoint is reachable for the actor rotating on the run:
+    // no points of actor's circle should be farther from the center than destination point
+    return (radius <= distToDest);
+}
diff --git a/apps/openmw/mwmechanics/aipackage.hpp b/apps/openmw/mwmechanics/aipackage.hpp
index c0df76c32..4960c14c3 100644
--- a/apps/openmw/mwmechanics/aipackage.hpp
+++ b/apps/openmw/mwmechanics/aipackage.hpp
@@ -97,6 +97,9 @@ namespace MWMechanics
 
             bool isTargetMagicallyHidden(const MWWorld::Ptr& target);
 
+            /// 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 ESM::Pathgrid::Point& dest);
+
         protected:
             /// Handles path building and shortcutting with obstacles avoiding
             /** \return If the actor has arrived at his destination **/
@@ -123,6 +126,8 @@ namespace MWMechanics
 
             osg::Vec3f mLastActorPos;
 
+            short mRotateOnTheRunChecks; // attempts to check rotation to the pathpoint on the run possibility
+
             bool mIsShortcutting;   // if shortcutting at the moment
             bool mShortcutProhibited; // shortcutting may be prohibited after unsuccessful attempt
             ESM::Pathgrid::Point mShortcutFailPos; // position of last shortcut fail