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

@@ -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());

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);
     };