Build path by navigator

apps/openmw/mwmechanics/aipackage.cpp

@@ -148,7 +148,9 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
         {
             if (wasShortcutting || doesPathNeedRecalc(dest, actor.getCell())) // if need to rebuild path
             {
-                mPathFinder.buildSyncedPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell()));
+                const auto playerHalfExtents = world->getHalfExtents(world->getPlayerPtr());
+                mPathFinder.buildPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell()),
+                    playerHalfExtents, getNavigatorFlags(actor));
                 mRotateOnTheRunChecks = 3;
 
                 // give priority to go directly on target if there is minimal opportunity

apps/openmw/mwmechanics/aiwander.cpp

@@ -50,7 +50,8 @@ namespace MWMechanics
 
     AiWander::AiWander(int distance, int duration, int timeOfDay, const std::vector<unsigned char>& idle, bool repeat):
         mDistance(distance), mDuration(duration), mRemainingDuration(duration), mTimeOfDay(timeOfDay), mIdle(idle),
-        mRepeat(repeat), mStoredInitialActorPosition(false), mInitialActorPosition(osg::Vec3f(0, 0, 0)), mHasDestination(false), mDestination(osg::Vec3f(0, 0, 0))
+        mRepeat(repeat), mStoredInitialActorPosition(false), mInitialActorPosition(osg::Vec3f(0, 0, 0)),
+        mHasDestination(false), mDestination(osg::Vec3f(0, 0, 0)), mUsePathgrid(false)
     {
         mIdle.resize(8, 0);
         init();
@@ -151,8 +152,18 @@ namespace MWMechanics
         // rebuild a path to it
         if (!mPathFinder.isPathConstructed() && mHasDestination)
         {
-            mPathFinder.buildSyncedPath(pos.asVec3(), mDestination, actor.getCell(),
+            if (mUsePathgrid)
-                getPathGridGraph(actor.getCell()));
+            {
+                mPathFinder.buildPathByPathgrid(pos.asVec3(), mDestination, actor.getCell(),
+                    getPathGridGraph(actor.getCell()));
+            }
+            else
+            {
+                const auto world = MWBase::Environment::get().getWorld();
+                const auto playerHalfExtents = world->getHalfExtents(world->getPlayerPtr());
+                mPathFinder.buildPath(pos.asVec3(), mDestination, actor.getCell(), getPathGridGraph(actor.getCell()),
+                    playerHalfExtents, getNavigatorFlags(actor));
+            }
 
             if (mPathFinder.isPathConstructed())
                 storage.setState(AiWanderStorage::Wander_Walking);
@@ -310,14 +321,17 @@ namespace MWMechanics
             if ((!isWaterCreature && !destinationIsAtWater(actor, mDestination)) ||
                 (isWaterCreature && !destinationThroughGround(currentPosition, mDestination)))
             {
-                mPathFinder.buildSyncedPath(currentPosition, destinationPosition, actor.getCell(),
+                const auto world = MWBase::Environment::get().getWorld();;
-                    getPathGridGraph(actor.getCell()));
+                const auto playerHalfExtents = world->getHalfExtents(world->getPlayerPtr());
+                mPathFinder.buildPath(currentPosition, destinationPosition, actor.getCell(),
+                    getPathGridGraph(actor.getCell()), playerHalfExtents, getNavigatorFlags(actor));
                 mPathFinder.addPointToPath(destinationPosition);
 
                 if (mPathFinder.isPathConstructed())
                 {
                     storage.setState(AiWanderStorage::Wander_Walking, true);
                     mHasDestination = true;
+                    mUsePathgrid = false;
                 }
                 return;
             }
@@ -595,12 +609,13 @@ namespace MWMechanics
 
         // don't take shortcuts for wandering
         const auto destVec3f = PathFinder::makeOsgVec3(dest);
-        mPathFinder.buildSyncedPath(start, destVec3f, actor.getCell(), getPathGridGraph(actor.getCell()));
+        mPathFinder.buildPathByPathgrid(start, destVec3f, actor.getCell(), getPathGridGraph(actor.getCell()));
 
         if (mPathFinder.isPathConstructed())
         {
             mDestination = destVec3f;
             mHasDestination = true;
+            mUsePathgrid = true;
             // Remove this node as an option and add back the previously used node (stops NPC from picking the same node):
             ESM::Pathgrid::Point temp = storage.mAllowedNodes[randNode];
             storage.mAllowedNodes.erase(storage.mAllowedNodes.begin() + randNode);

apps/openmw/mwmechanics/aiwander.hpp

@@ -164,6 +164,7 @@ namespace MWMechanics
 
             bool mHasDestination;
             osg::Vec3f mDestination;
+            bool mUsePathgrid;
 
             void getNeighbouringNodes(ESM::Pathgrid::Point dest, const MWWorld::CellStore* currentCell, ESM::Pathgrid::PointList& points);
 

apps/openmw/mwmechanics/pathfinding.cpp

@@ -124,13 +124,9 @@ namespace MWMechanics
      *    j = @.x in local coordinates (i.e. within the cell)
      *    k = @.x in world coordinates
      */
-    void PathFinder::buildPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
+    void PathFinder::buildPathByPathgridImpl(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-                               const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph)
+        const PathgridGraph& pathgridGraph, std::back_insert_iterator<std::deque<osg::Vec3f>> out)
     {
-        mConstructed = true;
-        mPath.clear();
-        mCell = cell;
-
         const auto pathgrid = pathgridGraph.getPathgrid();
 
         // Refer to AiWander reseach topic on openmw forums for some background.
@@ -138,7 +134,7 @@ namespace MWMechanics
         // physics take care of any blockages.
         if(!pathgrid || pathgrid->mPoints.empty())
         {
-            mPath.push_back(endPoint);
+            *out++ = endPoint;
             return;
         }
 
@@ -165,7 +161,7 @@ namespace MWMechanics
         float startTo1stNodeLength2 = distanceSquared(pathgrid->mPoints[startNode], startPointInLocalCoords);
         if ((startToEndLength2 < startTo1stNodeLength2) || (startToEndLength2 < endTolastNodeLength2))
         {
-            mPath.push_back(endPoint);
+            *out++ = endPoint;
             return;
         }
 
@@ -178,7 +174,7 @@ namespace MWMechanics
         {
             ESM::Pathgrid::Point temp(pathgrid->mPoints[startNode]);
             converter.toWorld(temp);
-            mPath.push_back(makeOsgVec3(temp));
+            *out++ = makeOsgVec3(temp);
         }
         else
         {
@@ -207,7 +203,7 @@ namespace MWMechanics
             }
 
             // convert supplied path to world coordinates
-            std::transform(path.begin(), path.end(), std::back_inserter(mPath),
+            std::transform(path.begin(), path.end(), out,
                 [&] (ESM::Pathgrid::Point& point)
                 {
                     converter.toWorld(point);
@@ -228,7 +224,7 @@ namespace MWMechanics
         //
         // The AI routines will have to deal with such situations.
         if(endNode.second)
-            mPath.push_back(endPoint);
+            *out++ = endPoint;
     }
 
     float PathFinder::getZAngleToNext(float x, float y) const
@@ -263,43 +259,40 @@ namespace MWMechanics
             mPath.pop_front();
     }
 
-    // see header for the rationale
+    void PathFinder::buildPathByPathgrid(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-    void PathFinder::buildSyncedPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
+        const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph)
-        const MWWorld::CellStore* cell, const MWMechanics::PathgridGraph& pathgridGraph)
     {
-        if (mPath.size() < 2)
+        mPath.clear();
-        {
+        mCell = cell;
-            // if path has one point, then it's the destination.
+
-            // don't need to worry about bad path for this case
+        buildPathByPathgridImpl(startPoint, endPoint, pathgridGraph, std::back_inserter(mPath));
-            buildPath(startPoint, endPoint, cell, pathgridGraph);
+
-        }
+        mConstructed = true;
-        else
-        {
-            const auto oldStart = getPath().front();
-            buildPath(startPoint, endPoint, cell, pathgridGraph);
-            if (mPath.size() >= 2)
-            {
-                // if 2nd waypoint of new path == 1st waypoint of old,
-                // delete 1st waypoint of new path.
-                if (mPath[1] == oldStart)
-                {
-                    mPath.pop_front();
-                }
-            }
-        }
     }
 
-    void PathFinder::buildPathByNavigator(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
+    void PathFinder::buildPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-        const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags)
+        const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents,
+        const DetourNavigator::Flags flags)
+    {
+        mPath.clear();
+        mCell = cell;
+
+        buildPathByNavigatorImpl(startPoint, endPoint, halfExtents, flags, std::back_inserter(mPath));
+
+        if (mPath.empty())
+            buildPathByPathgridImpl(startPoint, endPoint, pathgridGraph, std::back_inserter(mPath));
+
+        mConstructed = true;
+    }
+
+    void PathFinder::buildPathByNavigatorImpl(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
+        const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags,
+        std::back_insert_iterator<std::deque<osg::Vec3f>> out)
     {
         try
         {
-            mPath.clear();
-
             const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
-            navigator->findPath(halfExtents, startPoint, endPoint, flags, std::back_inserter(mPath));
+            navigator->findPath(halfExtents, startPoint, endPoint, flags, out);
-
-            mConstructed = true;
         }
         catch (const DetourNavigator::NavigatorException& exception)
         {

apps/openmw/mwmechanics/pathfinding.hpp

@@ -3,6 +3,7 @@
 
 #include <deque>
 #include <cassert>
+#include <iterator>
 
 #include <components/detournavigator/flags.hpp>
 #include <components/esm/defs.hpp>
@@ -70,8 +71,12 @@ namespace MWMechanics
                 mCell = nullptr;
             }
 
+            void buildPathByPathgrid(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
+                const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph);
+
             void buildPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-                           const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph);
+                const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents,
+                const DetourNavigator::Flags flags);
 
             /// Remove front point if exist and within tolerance
             void update(const osg::Vec3f& position, const float tolerance = DEFAULT_TOLERANCE);
@@ -106,16 +111,6 @@ namespace MWMechanics
                 return mCell;
             }
 
-            /** Synchronize new path with old one to avoid visiting 1 waypoint 2 times
-            @note
-                BuildPath() takes closest PathGrid point to NPC as first point of path.
-                This is undesirable if NPC has just passed a Pathgrid point, as this
-                makes the 2nd point of the new path == the 1st point of old path.
-                Which results in NPC "running in a circle" back to the just passed waypoint.
-             */
-            void buildSyncedPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-                const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph);
-
             void addPointToPath(const osg::Vec3f& point)
             {
                 mConstructed = true;
@@ -176,14 +171,18 @@ namespace MWMechanics
                 return closestIndex;
             }
 
-            void buildPathByNavigator(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-                const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags);
-
         private:
             bool mConstructed;
             std::deque<osg::Vec3f> mPath;
 
             const MWWorld::CellStore* mCell;
+
+            void buildPathByPathgridImpl(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
+                const PathgridGraph& pathgridGraph, std::back_insert_iterator<std::deque<osg::Vec3f>> out);
+
+            void buildPathByNavigatorImpl(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
+                const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags,
+                std::back_insert_iterator<std::deque<osg::Vec3f>> out);
     };
 }