From 1239667cb415835d2d4cb6f3aba941c92e4fecaa Mon Sep 17 00:00:00 2001
From: dteviot <dteviot@gmail.com>
Date: Sun, 5 Jul 2015 18:21:35 +1200
Subject: [PATCH] AiWander uses points between PathGrid points (Fixes #1317)

When there is only on PathGrid point within a NPC's wander distance, expand possible wander destinations by using positions between PathGrid points.
---
 apps/openmw/mwmechanics/aiwander.cpp    | 44 ++++++++++++++++++++++++-
 apps/openmw/mwmechanics/aiwander.hpp    |  4 +++
 apps/openmw/mwmechanics/pathfinding.hpp |  4 ++-
 3 files changed, 50 insertions(+), 2 deletions(-)

diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp
index 6f2c6969c..25ed4694b 100644
--- a/apps/openmw/mwmechanics/aiwander.cpp
+++ b/apps/openmw/mwmechanics/aiwander.cpp
@@ -216,7 +216,7 @@ namespace MWMechanics
         // Are we there yet?
         bool& chooseAction = storage.mChooseAction;
         if(walking &&
-           storage.mPathFinder.checkPathCompleted(pos.pos[0], pos.pos[1], 64.f))
+           storage.mPathFinder.checkPathCompleted(pos.pos[0], pos.pos[1]))
         {
             stopWalking(actor, storage);
             moveNow = false;
@@ -694,11 +694,19 @@ namespace MWMechanics
 
             // mAllowedNodes for this actor with pathgrid point indexes based on mDistance
             // NOTE: mPoints and mAllowedNodes are in local co-ordinates
+            int pointIndex = 0;
             for(unsigned int counter = 0; counter < pathgrid->mPoints.size(); counter++)
             {
                 osg::Vec3f nodePos(PathFinder::MakeOsgVec3(pathgrid->mPoints[counter]));
                 if((npcPos - nodePos).length2() <= mDistance * mDistance)
+                {
                     mAllowedNodes.push_back(pathgrid->mPoints[counter]);
+                    pointIndex = counter;
+                }
+            }
+            if (mAllowedNodes.size() == 1)
+            {
+                AddNonPathGridAllowedPoints(npcPos, pathgrid, pointIndex);
             }
             if(!mAllowedNodes.empty())
             {
@@ -708,6 +716,40 @@ namespace MWMechanics
         }
     }
 
+    // When only one path grid point in wander distance, 
+    // additional points for NPC to wander to are:
+    // 1. NPC's initial location
+    // 2. Partway along the path between the point and its connected points.
+    void AiWander::AddNonPathGridAllowedPoints(osg::Vec3f npcPos, const ESM::Pathgrid * pathGrid, int pointIndex)
+    {
+        mAllowedNodes.push_back(PathFinder::MakePathgridPoint(npcPos));
+        for (std::vector<ESM::Pathgrid::Edge>::const_iterator it = pathGrid->mEdges.begin(); it != pathGrid->mEdges.end(); ++it)
+        {
+            if (it->mV0 == pointIndex)
+            {
+                AddPointBetweenPathGridPoints(pathGrid->mPoints[it->mV0], pathGrid->mPoints[it->mV1]);
+            }
+        }
+    }
+
+    void AiWander::AddPointBetweenPathGridPoints(const ESM::Pathgrid::Point& start, const ESM::Pathgrid::Point& end)
+    {
+        osg::Vec3f vectorStart = PathFinder::MakeOsgVec3(start);
+        osg::Vec3f delta = PathFinder::MakeOsgVec3(end) - vectorStart;
+        float length = delta.length();
+        delta.normalize();
+
+        // destination must be far enough away that NPC will need to move to get there.
+        const int threshold = PathFinder::PathTolerance * 2;
+        int distance = std::max(mDistance / 2, threshold);
+        
+        // must not travel more than 1/2 way between waypoints,
+        // otherwise, NPC goes to far endpoint then comes back.  Looks weird.
+        distance = std::min(distance, static_cast<int>(length / 2));
+        delta *= distance;
+        mAllowedNodes.push_back(PathFinder::MakePathgridPoint(vectorStart + delta));
+    }
+
     void AiWander::SetCurrentNodeToClosestAllowedNode(osg::Vec3f npcPos)
     {
         float distanceToClosestNode = FLT_MAX;
diff --git a/apps/openmw/mwmechanics/aiwander.hpp b/apps/openmw/mwmechanics/aiwander.hpp
index 94758afc8..18f98cfd5 100644
--- a/apps/openmw/mwmechanics/aiwander.hpp
+++ b/apps/openmw/mwmechanics/aiwander.hpp
@@ -123,6 +123,10 @@ namespace MWMechanics
 
             void SetCurrentNodeToClosestAllowedNode(osg::Vec3f npcPos);
 
+            void AddNonPathGridAllowedPoints(osg::Vec3f npcPos, const ESM::Pathgrid * pathGrid, int pointIndex);
+
+            void AddPointBetweenPathGridPoints(const ESM::Pathgrid::Point& start, const ESM::Pathgrid::Point& end);
+
             /// lookup table for converting idleSelect value to groupName
             static const std::string sIdleSelectToGroupName[GroupIndex_MaxIdle - GroupIndex_MinIdle + 1];
     };
diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp
index 45d9dd973..a4886a840 100644
--- a/apps/openmw/mwmechanics/pathfinding.hpp
+++ b/apps/openmw/mwmechanics/pathfinding.hpp
@@ -19,6 +19,8 @@ namespace MWMechanics
         public:
             PathFinder();
 
+            static const int PathTolerance = 32;
+
             static float sgn(float val)
             {
                 if(val > 0)
@@ -35,7 +37,7 @@ namespace MWMechanics
 
             void clearPath();
 
-            bool checkPathCompleted(float x, float y, float tolerance=32.f);
+            bool checkPathCompleted(float x, float y, float tolerance = PathTolerance);
             ///< \Returns true if we are within \a tolerance units of the last path point.
 
             /// In degrees