Build path to first pathgrid node by navmesh

apps/openmw/mwmechanics/aiwander.cpp

@@ -448,7 +448,7 @@ namespace MWMechanics
         else
         {
             // have not yet reached the destination
-            evadeObstacles(actor, storage);
+            evadeObstacles(actor, duration, storage);
         }
     }
 
@@ -479,8 +479,17 @@ namespace MWMechanics
         storage.setState(AiWanderStorage::Wander_IdleNow);
     }
 
-    void AiWander::evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage)
+    void AiWander::evadeObstacles(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage)
     {
+        if (mUsePathgrid)
+        {
+            const auto halfExtents = MWBase::Environment::get().getWorld()->getHalfExtents(actor);
+            const float actorTolerance = 2 * actor.getClass().getSpeed(actor) * duration
+                    + 1.2 * std::max(halfExtents.x(), halfExtents.y());
+            const float pointTolerance = std::max(MIN_TOLERANCE, actorTolerance);
+            mPathFinder.buildPathByNavMeshToNextPoint(actor, halfExtents, getNavigatorFlags(actor), pointTolerance);
+        }
+
         if (mObstacleCheck.isEvading())
         {
             // first check if we're walking into a door

apps/openmw/mwmechanics/aiwander.hpp

@@ -137,7 +137,7 @@ namespace MWMechanics
             short unsigned getRandomIdle();
             void setPathToAnAllowedNode(const MWWorld::Ptr& actor, AiWanderStorage& storage, const ESM::Position& actorPos);
             void playGreetingIfPlayerGetsTooClose(const MWWorld::Ptr& actor, AiWanderStorage& storage);
-            void evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage);
+            void evadeObstacles(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage);
             void turnActorToFacePlayer(const osg::Vec3f& actorPosition, const osg::Vec3f& playerPosition, AiWanderStorage& storage);
             void doPerFrameActionsForState(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage);
             void onIdleStatePerFrameActions(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage);

apps/openmw/mwmechanics/pathfinding.cpp

@@ -73,6 +73,13 @@ namespace
     {
         return sqrDistance(osg::Vec2f(lhs.x(), lhs.y()), osg::Vec2f(rhs.x(), rhs.y()));
     }
+
+    float getPathStepSize(const MWWorld::ConstPtr& actor)
+    {
+        const auto world = MWBase::Environment::get().getWorld();
+        const auto realHalfExtents = world->getHalfExtents(actor);
+        return 2 * std::max(realHalfExtents.x(), realHalfExtents.y());
+    }
 }
 
 namespace MWMechanics
@@ -320,8 +327,7 @@ namespace MWMechanics
         try
         {
             const auto world = MWBase::Environment::get().getWorld();
-            const auto realHalfExtents = world->getHalfExtents(actor); // This may differ from halfExtents argument
-            const auto stepSize = 2 * std::max(realHalfExtents.x(), realHalfExtents.y());
+            const auto stepSize = getPathStepSize(actor);
             const auto navigator = world->getNavigator();
             navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, out);
         }
@@ -333,4 +339,39 @@ namespace MWMechanics
                 << DetourNavigator::WriteFlags {flags} << ")";
         }
     }
+
+    void PathFinder::buildPathByNavMeshToNextPoint(const MWWorld::ConstPtr& actor, const osg::Vec3f& halfExtents,
+        const DetourNavigator::Flags flags, const float pointTolerance)
+    {
+        if (mPath.empty())
+            return;
+
+        const auto stepSize = getPathStepSize(actor);
+        const auto startPoint = actor.getRefData().getPosition().asVec3();
+
+        if (sqrDistanceIgnoreZ(mPath.front(), startPoint) <= 4 * stepSize * stepSize)
+            return;
+
+        try
+        {
+            const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
+            std::deque<osg::Vec3f> prePath;
+            navigator->findPath(halfExtents, stepSize, startPoint, mPath.front(), flags, std::back_inserter(prePath));
+
+            while (!prePath.empty() && sqrDistanceIgnoreZ(prePath.front(), startPoint) < stepSize * stepSize)
+                prePath.pop_front();
+
+            while (!prePath.empty() && sqrDistanceIgnoreZ(prePath.back(), mPath.front()) < stepSize * stepSize)
+                prePath.pop_back();
+
+            std::copy(prePath.rbegin(), prePath.rend(), std::front_inserter(mPath));
+        }
+        catch (const DetourNavigator::NavigatorException& exception)
+        {
+            Log(Debug::Debug) << "Build path by navigator exception: \"" << exception.what()
+                << "\" for \"" << actor.getClass().getName(actor) << "\" (" << actor.getBase()
+                << ") from " << startPoint << " to " << mPath.front() << " with flags ("
+                << DetourNavigator::WriteFlags {flags} << ")";
+        }
+    }
 }

apps/openmw/mwmechanics/pathfinding.hpp

@@ -84,6 +84,9 @@ namespace MWMechanics
                 const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents,
                 const DetourNavigator::Flags flags);
 
+            void buildPathByNavMeshToNextPoint(const MWWorld::ConstPtr& actor, const osg::Vec3f& halfExtents,
+                const DetourNavigator::Flags flags, const float pointTolerance);
+
             /// Remove front point if exist and within tolerance
             void update(const osg::Vec3f& position, const float pointTolerance, const float destinationTolerance);