Remove unnecessary loops from path

This prevents actors going back when a new shortest path includes a point
behind them where they were right before. Such situation can happen when path
includes off mesh connection. Resulting cost of such path can be lower than
the real one because off mesh connections are straight lines and walking
surface usually is not a plane but a surface.

Skip to path point where distance from current position to the line between
previous and this point is less than point tolerance. Which means actor is
standing very close to the edge between those points. Additionally check by
navmesh raycasting to make sure there is actually a valid path.

apps/openmw/mwmechanics/pathfinding.cpp

@@ -6,6 +6,7 @@
 #include <components/detournavigator/navigatorutils.hpp>
 #include <components/debug/debuglog.hpp>
 #include <components/misc/coordinateconverter.hpp>
+#include <components/misc/math.hpp>
 
 #include "../mwbase/world.hpp"
 #include "../mwbase/environment.hpp"
@@ -315,12 +316,13 @@ namespace MWMechanics
         while (mPath.size() > 1 && sqrDistanceIgnoreZ(mPath.front(), position) < pointTolerance * pointTolerance)
             mPath.pop_front();
 
+        const IsValidShortcut isValidShortcut {
+            MWBase::Environment::get().getWorld()->getNavigator(),
+            halfExtents, flags
+        };
+
         if (shortenIfAlmostStraight)
         {
-            const IsValidShortcut isValidShortcut {
-                MWBase::Environment::get().getWorld()->getNavigator(),
-                halfExtents, flags
-            };
             while (mPath.size() > 2 && isAlmostStraight(mPath[0], mPath[1], mPath[2], pointTolerance)
                    && isValidShortcut(mPath[0], mPath[2]))
                 mPath.erase(mPath.begin() + 1);
@@ -329,6 +331,19 @@ namespace MWMechanics
                 mPath.pop_front();
         }
 
+        if (mPath.size() > 1)
+        {
+            std::size_t begin = 0;
+            for (std::size_t i = 1; i < mPath.size(); ++i)
+            {
+                const float sqrDistance = Misc::getVectorToLine(position, mPath[i - 1], mPath[i]).length2();
+                if (sqrDistance < pointTolerance * pointTolerance && isValidShortcut(position, mPath[i]))
+                    begin = i;
+            }
+            for (std::size_t i = 0; i < begin; ++i)
+                mPath.pop_front();
+        }
+
         if (mPath.size() == 1)
         {
             float distSqr;

components/misc/math.hpp

@@ -0,0 +1,16 @@
+#ifndef OPENMW_COMPONENTS_MISC_MATH_H
+#define OPENMW_COMPONENTS_MISC_MATH_H
+
+#include <osg/Vec3f>
+
+namespace Misc
+{
+    inline osg::Vec3f getVectorToLine(const osg::Vec3f& position, const osg::Vec3f& a, const osg::Vec3f& b)
+    {
+        osg::Vec3f direction = b - a;
+        direction.normalize();
+        return (position - a) ^ direction;
+    }
+}
+
+#endif