From 7ec7c57879b8d5384c69879b55074abc075f1534 Mon Sep 17 00:00:00 2001
From: elsid <elsid.mail@gmail.com>
Date: Sun, 23 Jan 2022 19:24:36 +0100
Subject: [PATCH] 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 | 23 +++++++++++++++++++----
 components/misc/math.hpp                | 16 ++++++++++++++++
 2 files changed, 35 insertions(+), 4 deletions(-)
 create mode 100644 components/misc/math.hpp

diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp
index e727d8585d..1cc4526193 100644
--- a/apps/openmw/mwmechanics/pathfinding.cpp
+++ b/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;
diff --git a/components/misc/math.hpp b/components/misc/math.hpp
new file mode 100644
index 0000000000..27e29675f2
--- /dev/null
+++ b/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