From be6ee927b906313cc7a19ed4298da725c22aebbd Mon Sep 17 00:00:00 2001
From: dteviot <dteviot@gmail.co.nz>
Date: Sat, 28 Mar 2015 20:05:54 +1300
Subject: [PATCH] AiWander, use closest two points if distance is too small
 (Fixes #1317)

In AiWander, if wander distance is set too small to get two points, take the closest two points.
---
 apps/openmw/mwmechanics/aiwander.cpp | 30 ++++++++++++++++++++++++++--
 apps/openmw/mwmechanics/aiwander.hpp |  7 +++++++
 2 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp
index d277b1249..560e756ce 100644
--- a/apps/openmw/mwmechanics/aiwander.cpp
+++ b/apps/openmw/mwmechanics/aiwander.cpp
@@ -686,7 +686,8 @@ namespace MWMechanics
         // If there is no path this actor doesn't go anywhere. See:
         // https://forum.openmw.org/viewtopic.php?t=1556
         // http://www.fliggerty.com/phpBB3/viewtopic.php?f=30&t=5833
-        if(!pathgrid || pathgrid->mPoints.empty())
+        // Note: In order to wander, need at least two points.
+        if(!pathgrid || (pathgrid->mPoints.size() < 2))
             mDistance = 0;
 
         // A distance value passed into the constructor indicates how far the
@@ -730,12 +731,37 @@ namespace MWMechanics
                 }
                 mCurrentNode = mAllowedNodes[index];
                 mAllowedNodes.erase(mAllowedNodes.begin() + index);
+            }
+            
+            // In vanilla Morrowind, sometimes distance is too small to include at least two points,
+            // in which case, we will take the two closest points regardless of the wander distance
+            // This is a backup option, as std::sort is potentially O(n^2) in time.
+            if (mAllowedNodes.empty())
+            {
+                // Start with list of PathGrid nodes, sorted by distance from actor
+                std::vector<PathDistance> nodeDistances;
+                for (unsigned int counter = 0; counter < pathgrid->mPoints.size(); counter++)
+                {
+                    float distance = npcPos.squaredDistance(PathFinder::MakeOgreVector3(pathgrid->mPoints[counter]));
+                    nodeDistances.push_back(std::make_pair(distance, &pathgrid->mPoints.at(counter)));
+                }
+                std::sort(nodeDistances.begin(), nodeDistances.end(), sortByDistance);
 
-                mStoredAvailableNodes = true; // set only if successful in finding allowed nodes
+                // make closest node the current node
+                mCurrentNode = *nodeDistances[0].second;
+
+                // give Actor a 2nd node to walk to
+                mAllowedNodes.push_back(*nodeDistances[1].second);
             }
+            mStoredAvailableNodes = true; // set only if successful in finding allowed nodes
         }
     }
 
+    bool AiWander::sortByDistance(const PathDistance& left, const PathDistance& right)
+    {
+        return left.first < right.first;
+    }
+
     void AiWander::writeState(ESM::AiSequence::AiSequence &sequence) const
     {
         std::auto_ptr<ESM::AiSequence::AiWander> wander(new ESM::AiSequence::AiWander());
diff --git a/apps/openmw/mwmechanics/aiwander.hpp b/apps/openmw/mwmechanics/aiwander.hpp
index 7e138c001..7f8fc5088 100644
--- a/apps/openmw/mwmechanics/aiwander.hpp
+++ b/apps/openmw/mwmechanics/aiwander.hpp
@@ -122,6 +122,13 @@ namespace MWMechanics
 
             /// lookup table for converting idleSelect value to groupName
             static const std::string sIdleSelectToGroupName[GroupIndex_MaxIdle - GroupIndex_MinIdle + 1];
+
+            /// record distances of pathgrid point nodes to actor
+            /// first value is distance between actor and node, second value is PathGrid node
+            typedef std::pair<float, const ESM::Pathgrid::Point*> PathDistance;
+
+            /// used to sort array of PathDistance objects into ascending order
+            static bool sortByDistance(const PathDistance& left, const PathDistance& right);
     };