Make AiPursue path destination to be as close as possible to target

Even when target is not reachable actor will try to run there either because
target navmesh polygon is selected within extended area or because partial path
is built to the closest possible polygon.

apps/openmw/mwmechanics/aicombat.cpp

@@ -269,7 +269,8 @@ namespace MWMechanics
             const auto navigatorFlags = getNavigatorFlags(actor);
             const auto areaCosts = getAreaCosts(actor);
             const auto pathGridGraph = getPathGridGraph(actor.getCell());
-            mPathFinder.buildPath(actor, vActorPos, vTargetPos, actor.getCell(), pathGridGraph, halfExtents, navigatorFlags, areaCosts);
+            mPathFinder.buildPath(actor, vActorPos, vTargetPos, actor.getCell(), pathGridGraph, halfExtents,
+                                  navigatorFlags, areaCosts, storage.mAttackRange, PathType::Full);
 
             if (!mPathFinder.isPathConstructed())
             {
@@ -281,7 +282,8 @@ namespace MWMechanics
                 if (hit.has_value() && (*hit - vTargetPos).length() <= rangeAttack)
                 {
                     // If the point is close enough, try to find a path to that point.
-                    mPathFinder.buildPath(actor, vActorPos, *hit, actor.getCell(), pathGridGraph, halfExtents, navigatorFlags, areaCosts);
+                    mPathFinder.buildPath(actor, vActorPos, *hit, actor.getCell(), pathGridGraph, halfExtents,
+                                          navigatorFlags, areaCosts, storage.mAttackRange, PathType::Full);
                     if (mPathFinder.isPathConstructed())
                     {
                         // If path to that point is found use it as custom destination.

apps/openmw/mwmechanics/aipackage.cpp

@@ -88,7 +88,8 @@ void MWMechanics::AiPackage::reset()
     mObstacleCheck.clear();
 }
 
-bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance)
+bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration,
+                                    float destTolerance, float endTolerance, PathType pathType)
 {
     const Misc::TimerStatus timerStatus = mReaction.update(duration);
 
@@ -133,7 +134,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
             {
                 const auto pathfindingHalfExtents = world->getPathfindingHalfExtents(actor);
                 mPathFinder.buildLimitedPath(actor, position, dest, actor.getCell(), getPathGridGraph(actor.getCell()),
-                    pathfindingHalfExtents, getNavigatorFlags(actor), getAreaCosts(actor));
+                    pathfindingHalfExtents, getNavigatorFlags(actor), getAreaCosts(actor), endTolerance, pathType);
                 mRotateOnTheRunChecks = 3;
 
                 // give priority to go directly on target if there is minimal opportunity

apps/openmw/mwmechanics/aipackage.hpp

@@ -129,7 +129,8 @@ namespace MWMechanics
         protected:
             /// Handles path building and shortcutting with obstacles avoiding
             /** \return If the actor has arrived at his destination **/
-            bool pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance = 0.0f);
+            bool pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration,
+                        float destTolerance = 0.0f, float endTolerance = 0.0f, PathType pathType = PathType::Full);
 
             /// Check if there aren't any obstacles along the path to make shortcut possible
             /// If a shortcut is possible then path will be cleared and filled with the destination point.

apps/openmw/mwmechanics/aipursue.cpp

@@ -53,7 +53,7 @@ bool AiPursue::execute (const MWWorld::Ptr& actor, CharacterController& characte
     const float pathTolerance = 100.f;
 
     // check the true distance in case the target is far away in Z-direction
-    bool reached = pathTo(actor, dest, duration, pathTolerance) &&
+    bool reached = pathTo(actor, dest, duration, pathTolerance, (actorPos - dest).length(), PathType::Partial) &&
                    std::abs(dest.z() - actorPos.z()) < pathTolerance;
 
     if (reached)

apps/openmw/mwmechanics/aiwander.cpp

@@ -201,8 +201,10 @@ namespace MWMechanics
             else
             {
                 const osg::Vec3f halfExtents = MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(actor);
+                constexpr float endTolerance = 0;
                 mPathFinder.buildPath(actor, pos.asVec3(), mDestination, actor.getCell(),
-                    getPathGridGraph(actor.getCell()), halfExtents, getNavigatorFlags(actor), getAreaCosts(actor));
+                    getPathGridGraph(actor.getCell()), halfExtents, getNavigatorFlags(actor), getAreaCosts(actor),
+                    endTolerance, PathType::Full);
             }
 
             if (mPathFinder.isPathConstructed())
@@ -361,11 +363,13 @@ namespace MWMechanics
             if (isAreaOccupiedByOtherActor(actor, mDestination))
                 continue;
 
+            constexpr float endTolerance = 0;
+
             if (isWaterCreature || isFlyingCreature)
                 mPathFinder.buildStraightPath(mDestination);
             else
                 mPathFinder.buildPathByNavMesh(actor, currentPosition, mDestination, halfExtents, navigatorFlags,
-                                               areaCosts);
+                                               areaCosts, endTolerance, PathType::Full);
 
             if (mPathFinder.isPathConstructed())
             {

apps/openmw/mwmechanics/pathfinding.cpp

@@ -364,13 +364,13 @@ namespace MWMechanics
 
     void PathFinder::buildPathByNavMesh(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint,
         const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags,
-        const DetourNavigator::AreaCosts& areaCosts)
+        const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType)
     {
         mPath.clear();
 
         // If it's not possible to build path over navmesh due to disabled navmesh generation fallback to straight path
         DetourNavigator::Status status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags,
-                                                                  areaCosts, std::back_inserter(mPath));
+            areaCosts, endTolerance, pathType, std::back_inserter(mPath));
 
         if (status != DetourNavigator::Status::Success)
             mPath.clear();
@@ -383,7 +383,8 @@ namespace MWMechanics
 
     void PathFinder::buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
         const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents,
-        const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts)
+        const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
+        PathType pathType)
     {
         mPath.clear();
         mCell = cell;
@@ -392,7 +393,8 @@ namespace MWMechanics
 
         if (!actor.getClass().isPureWaterCreature(actor) && !actor.getClass().isPureFlyingCreature(actor))
         {
-            status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, areaCosts, std::back_inserter(mPath));
+            status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, areaCosts,
+                                              endTolerance, pathType, std::back_inserter(mPath));
             if (status != DetourNavigator::Status::Success)
                 mPath.clear();
         }
@@ -400,7 +402,7 @@ namespace MWMechanics
         if (status != DetourNavigator::Status::NavMeshNotFound && mPath.empty())
         {
             status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents,
-                                     flags | DetourNavigator::Flag_usePathgrid, areaCosts, std::back_inserter(mPath));
+                flags | DetourNavigator::Flag_usePathgrid, areaCosts, endTolerance, pathType, std::back_inserter(mPath));
             if (status != DetourNavigator::Status::Success)
                 mPath.clear();
         }
@@ -416,12 +418,17 @@ namespace MWMechanics
 
     DetourNavigator::Status PathFinder::buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint,
         const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags,
-        const DetourNavigator::AreaCosts& areaCosts, std::back_insert_iterator<std::deque<osg::Vec3f>> out)
+        const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType,
+        std::back_insert_iterator<std::deque<osg::Vec3f>> out)
     {
         const auto world = MWBase::Environment::get().getWorld();
         const auto stepSize = getPathStepSize(actor);
         const auto navigator = world->getNavigator();
-        const auto status = navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, areaCosts, out);
+        const auto status = navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, areaCosts,
+                                                endTolerance, out);
+
+        if (pathType == PathType::Partial && status == DetourNavigator::Status::PartialPath)
+            return DetourNavigator::Status::Success;
 
         if (status != DetourNavigator::Status::Success)
         {
@@ -449,8 +456,9 @@ namespace MWMechanics
         const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
         std::deque<osg::Vec3f> prePath;
         auto prePathInserter = std::back_inserter(prePath);
+        const float endTolerance = 0;
         const auto status = navigator->findPath(halfExtents, stepSize, startPoint, mPath.front(), flags, areaCosts,
-                                                prePathInserter);
+                                                endTolerance, prePathInserter);
 
         if (status == DetourNavigator::Status::NavMeshNotFound)
             return;
@@ -475,7 +483,8 @@ namespace MWMechanics
 
     void PathFinder::buildLimitedPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
         const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents,
-        const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts)
+        const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
+        PathType pathType)
     {
         const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
         const auto maxDistance = std::min(
@@ -485,8 +494,9 @@ namespace MWMechanics
         const auto startToEnd = endPoint - startPoint;
         const auto distance = startToEnd.length();
         if (distance <= maxDistance)
-            return buildPath(actor, startPoint, endPoint, cell, pathgridGraph, halfExtents, flags, areaCosts);
+            return buildPath(actor, startPoint, endPoint, cell, pathgridGraph, halfExtents, flags, areaCosts,
+                             endTolerance, pathType);
         const auto end = startPoint + startToEnd * maxDistance / distance;
-        buildPath(actor, startPoint, end, cell, pathgridGraph, halfExtents, flags, areaCosts);
+        buildPath(actor, startPoint, end, cell, pathgridGraph, halfExtents, flags, areaCosts, endTolerance, pathType);
     }
 }

apps/openmw/mwmechanics/pathfinding.hpp

@@ -70,6 +70,12 @@ namespace MWMechanics
     // magnitude of pits/obstacles is defined by PATHFIND_Z_REACH
     bool checkWayIsClear(const osg::Vec3f& from, const osg::Vec3f& to, float offsetXY);
 
+    enum class PathType
+    {
+        Full,
+        Partial,
+    };
+
     class PathFinder
     {
         public:
@@ -93,18 +99,20 @@ namespace MWMechanics
 
             void buildPathByNavMesh(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint,
                 const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags,
-                const DetourNavigator::AreaCosts& areaCosts);
+                const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType);
 
             void buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
                 const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents,
-                const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts);
+                const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
+                PathType pathType);
 
             void buildPathByNavMeshToNextPoint(const MWWorld::ConstPtr& actor, const osg::Vec3f& halfExtents,
                 const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts);
 
             void buildLimitedPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
                 const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents,
-                const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts);
+                const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
+                PathType pathType);
 
             /// Remove front point if exist and within tolerance
             void update(const osg::Vec3f& position, float pointTolerance, float destinationTolerance,
@@ -212,7 +220,7 @@ namespace MWMechanics
 
             [[nodiscard]] DetourNavigator::Status buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor,
                 const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents,
-                const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts,
+                const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType,
                 std::back_insert_iterator<std::deque<osg::Vec3f>> out);
     };
 }

apps/openmw_test_suite/detournavigator/navigator.cpp

@@ -46,6 +46,7 @@ namespace
         const osg::Vec2i mCellPosition {0, 0};
         const int mHeightfieldTileSize = ESM::Land::REAL_SIZE / (ESM::Land::LAND_SIZE - 1);
         const osg::Vec3f mShift {0, 0, 0};
+        const float mEndTolerance = 0;
 
         DetourNavigatorNavigatorTest()
             : mPlayerPosition(0, 0, 0)
@@ -135,7 +136,7 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty)
     {
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut),
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
                   Status::NavMeshNotFound);
         EXPECT_EQ(mPath, std::deque<osg::Vec3f>());
     }
@@ -143,7 +144,7 @@ namespace
     TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception)
     {
         mNavigator->addAgent(mAgentHalfExtents);
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut),
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
                   Status::StartPolygonNotFound);
     }
 
@@ -152,7 +153,7 @@ namespace
         mNavigator->addAgent(mAgentHalfExtents);
         mNavigator->addAgent(mAgentHalfExtents);
         mNavigator->removeAgent(mAgentHalfExtents);
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut),
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
                   Status::StartPolygonNotFound);
     }
 
@@ -172,7 +173,8 @@ namespace
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent);
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204.0000152587890625, 204, 1.99998295307159423828125),
@@ -219,7 +221,8 @@ namespace
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 1.99998295307159423828125),
@@ -252,7 +255,8 @@ namespace
 
         mPath.clear();
         mOut = std::back_inserter(mPath);
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 1.99998295307159423828125),
@@ -301,7 +305,8 @@ namespace
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 1.99998295307159423828125),
@@ -337,7 +342,8 @@ namespace
 
         mPath.clear();
         mOut = std::back_inserter(mPath);
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 1.99998295307159423828125),
@@ -393,7 +399,8 @@ namespace
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 1.999981403350830078125),
@@ -479,7 +486,8 @@ namespace
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 1.99997997283935546875),
@@ -530,7 +538,8 @@ namespace
         mEnd.x() = 0;
         mEnd.z() = 300;
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(0, 204, 185.33331298828125),
@@ -574,7 +583,7 @@ namespace
         mStart.x() = 0;
         mEnd.x() = 0;
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mOut),
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut),
                   Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
@@ -619,7 +628,7 @@ namespace
         mStart.x() = 0;
         mEnd.x() = 0;
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mOut),
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut),
                   Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
@@ -664,7 +673,8 @@ namespace
         mStart.x() = 0;
         mEnd.x() = 0;
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(0, 204, -98.000030517578125),
@@ -712,7 +722,8 @@ namespace
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 1.99998295307159423828125),
@@ -764,7 +775,8 @@ namespace
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 1.99998295307159423828125),
@@ -858,7 +870,8 @@ namespace
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::allJobsDone);
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 1.99998295307159423828125),
@@ -1006,7 +1019,8 @@ namespace
         mNavigator->update(mPlayerPosition);
         mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent);
 
-        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(
             Vec3fEq(-204, 204, 101.99999237060546875),
@@ -1033,4 +1047,98 @@ namespace
             Vec3fEq(204, -204, 101.99999237060546875)
         )) << mPath;
     }
+
+    TEST_F(DetourNavigatorNavigatorTest, for_not_reachable_destination_find_path_should_provide_partial_path)
+    {
+        const std::array<float, 5 * 5> heightfieldData {{
+            0,   0,    0,    0,    0,
+            0, -25,  -25,  -25,  -25,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+        }};
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
+
+        CollisionShapeInstance compound(std::make_unique<btCompoundShape>());
+        compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(204, -204, 0)),
+                                       new btBoxShape(btVector3(200, 200, 1000)));
+
+        mNavigator->addAgent(mAgentHalfExtents);
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
+        mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity());
+        mNavigator->update(mPlayerPosition);
+        mNavigator->wait(mListener, WaitConditionType::allJobsDone);
+
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                  Status::PartialPath);
+
+        EXPECT_THAT(mPath, ElementsAre(
+            Vec3fEq(-204, 204, -2.666739940643310546875),
+            Vec3fEq(-193.730682373046875, 177.59320068359375, -3.9535934925079345703125),
+            Vec3fEq(-183.4613800048828125, 151.1864166259765625, -5.240451335906982421875),
+            Vec3fEq(-173.1920623779296875, 124.77962493896484375, -6.527309417724609375),
+            Vec3fEq(-162.922760009765625, 98.37282562255859375, -7.814167022705078125),
+            Vec3fEq(-152.6534423828125, 71.96602630615234375, -9.898590087890625),
+            Vec3fEq(-142.384124755859375, 45.559230804443359375, -17.641445159912109375),
+            Vec3fEq(-132.1148223876953125, 19.152431488037109375, -25.3843059539794921875),
+            Vec3fEq(-121.8455047607421875, -7.254369258880615234375, -27.97742462158203125),
+            Vec3fEq(-111.57619476318359375, -33.66117095947265625, -16.974590301513671875),
+            Vec3fEq(-101.30689239501953125, -60.06797027587890625, -5.9717559814453125),
+            Vec3fEq(-91.0375823974609375, -86.47476959228515625, -2.6667339801788330078125),
+            Vec3fEq(-80.76827239990234375, -112.88156890869140625, -2.6667339801788330078125),
+            Vec3fEq(-70.49897003173828125, -139.2883758544921875, -2.6667339801788330078125),
+            Vec3fEq(-60.229663848876953125, -165.6951751708984375, -2.6667339801788330078125),
+            Vec3fEq(-49.96035003662109375, -192.1019744873046875, -2.6667339801788330078125),
+            Vec3fEq(-45.333343505859375, -204, -2.6667339801788330078125)
+        )) << mPath;
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, end_tolerance_should_extent_available_destinations)
+    {
+        const std::array<float, 5 * 5> heightfieldData {{
+            0,   0,    0,    0,    0,
+            0, -25,  -25,  -25,  -25,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+        }};
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
+
+        CollisionShapeInstance compound(std::make_unique<btCompoundShape>());
+        compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(204, -204, 0)),
+                                       new btBoxShape(btVector3(100, 100, 1000)));
+
+        mNavigator->addAgent(mAgentHalfExtents);
+        mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
+        mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity());
+        mNavigator->update(mPlayerPosition);
+        mNavigator->wait(mListener, WaitConditionType::allJobsDone);
+
+        const float endTolerance = 1000.0f;
+
+        EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, endTolerance, mOut),
+                  Status::Success);
+
+        EXPECT_THAT(mPath, ElementsAre(
+            Vec3fEq(-204, 204, -2.666739940643310546875),
+            Vec3fEq(-188.745635986328125, 180.1236114501953125, -4.578275203704833984375),
+            Vec3fEq(-173.49127197265625, 156.247222900390625, -6.489814281463623046875),
+            Vec3fEq(-158.2369232177734375, 132.370819091796875, -8.4013538360595703125),
+            Vec3fEq(-142.9825592041015625, 108.49443817138671875, -10.31289386749267578125),
+            Vec3fEq(-127.7281951904296875, 84.6180419921875, -17.4810466766357421875),
+            Vec3fEq(-112.47383880615234375, 60.7416534423828125, -27.6026020050048828125),
+            Vec3fEq(-97.21947479248046875, 36.865261077880859375, -37.724163055419921875),
+            Vec3fEq(-81.965118408203125, 12.98886966705322265625, -47.84572601318359375),
+            Vec3fEq(-66.71075439453125, -10.887523651123046875, -46.691577911376953125),
+            Vec3fEq(-51.45639801025390625, -34.763916015625, -32.085445404052734375),
+            Vec3fEq(-36.202037811279296875, -58.640308380126953125, -28.5217914581298828125),
+            Vec3fEq(-20.947673797607421875, -82.5167083740234375, -32.16143035888671875),
+            Vec3fEq(-5.693310260772705078125, -106.393096923828125, -35.8010711669921875),
+            Vec3fEq(9.56105327606201171875, -130.2694854736328125, -29.6399688720703125),
+            Vec3fEq(24.8154163360595703125, -154.1458740234375, -17.6428318023681640625),
+            Vec3fEq(40.0697784423828125, -178.0222625732421875, -10.46006107330322265625),
+            Vec3fEq(55.3241424560546875, -201.8986663818359375, -3.297139644622802734375),
+            Vec3fEq(56.66666412353515625, -204, -2.6667373180389404296875)
+        )) << mPath;
+    }
 }

components/detournavigator/debug.hpp

@@ -34,6 +34,7 @@ namespace DetourNavigator
         switch (value)
         {
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(Success)
+            OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(PartialPath)
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(NavMeshNotFound)
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(StartPolygonNotFound)
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(EndPolygonNotFound)

components/detournavigator/findrandompointaroundcircle.cpp

@@ -19,7 +19,7 @@ namespace DetourNavigator
         dtQueryFilter queryFilter;
         queryFilter.setIncludeFlags(includeFlags);
 
-        dtPolyRef startRef = findNearestPolyExpanding(navMeshQuery, queryFilter, start, halfExtents);
+        dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, halfExtents * 4);
         if (startRef == 0)
             return std::optional<osg::Vec3f>();
 

components/detournavigator/findsmoothpath.cpp

@@ -146,16 +146,13 @@ namespace DetourNavigator
         return result;
     }
 
-    dtPolyRef findNearestPolyExpanding(const dtNavMeshQuery& query, const dtQueryFilter& filter,
+    dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter,
             const osg::Vec3f& center, const osg::Vec3f& halfExtents)
     {
         dtPolyRef ref = 0;
-        for (int i = 0; i < 3; ++i)
+        const dtStatus status = query.findNearestPoly(center.ptr(), halfExtents.ptr(), &filter, &ref, nullptr);
-        {
+        if (!dtStatusSucceed(status))
-            const dtStatus status = query.findNearestPoly(center.ptr(), (halfExtents * (1 << i)).ptr(), &filter, &ref, nullptr);
+            return 0;
-            if (!dtStatusFailed(status) && ref != 0)
-                break;
-        }
         return ref;
     }
 }

components/detournavigator/findsmoothpath.hpp

@@ -99,7 +99,7 @@ namespace DetourNavigator
         return dtStatusSucceed(status);
     }
 
-    dtPolyRef findNearestPolyExpanding(const dtNavMeshQuery& query, const dtQueryFilter& filter,
+    dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter,
             const osg::Vec3f& center, const osg::Vec3f& halfExtents);
 
     struct MoveAlongSurfaceResult
@@ -256,7 +256,7 @@ namespace DetourNavigator
     template <class OutputIterator>
     Status findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, const float stepSize,
             const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts,
-            const Settings& settings, OutputIterator& out)
+            const Settings& settings, float endTolerance, OutputIterator& out)
     {
         dtNavMeshQuery navMeshQuery;
         if (!initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes))
@@ -269,11 +269,15 @@ namespace DetourNavigator
         queryFilter.setAreaCost(AreaType_pathgrid, areaCosts.mPathgrid);
         queryFilter.setAreaCost(AreaType_ground, areaCosts.mGround);
 
-        dtPolyRef startRef = findNearestPolyExpanding(navMeshQuery, queryFilter, start, halfExtents);
+        constexpr float polyDistanceFactor = 4;
+        const osg::Vec3f polyHalfExtents = halfExtents * polyDistanceFactor;
+
+        const dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, polyHalfExtents);
         if (startRef == 0)
             return Status::StartPolygonNotFound;
 
-        dtPolyRef endRef = findNearestPolyExpanding(navMeshQuery, queryFilter, end, halfExtents);
+        const dtPolyRef endRef = findNearestPoly(navMeshQuery, queryFilter, end,
+            polyHalfExtents + osg::Vec3f(endTolerance, endTolerance, endTolerance));
         if (endRef == 0)
             return Status::EndPolygonNotFound;
 
@@ -283,12 +287,18 @@ namespace DetourNavigator
         if (!polygonPath)
             return Status::FindPathOverPolygonsFailed;
 
-        if (polygonPath->empty() || polygonPath->back() != endRef)
+        if (polygonPath->empty())
             return Status::Success;
 
+        const bool partialPath = polygonPath->back() != endRef;
         auto outTransform = OutputTransformIterator<OutputIterator>(out, settings);
-        return makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize, std::move(*polygonPath),
+        const Status smoothStatus = makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize,
-            settings.mMaxSmoothPathSize, outTransform);
+                                                   std::move(*polygonPath), settings.mMaxSmoothPathSize, outTransform);
+
+        if (smoothStatus != Status::Success)
+            return smoothStatus;
+
+        return partialPath ? Status::PartialPath : Status::Success;
     }
 }
 

components/detournavigator/navigator.hpp

@@ -173,13 +173,14 @@ namespace DetourNavigator
          * @param end path at given point.
          * @param includeFlags setup allowed surfaces for actor to walk.
          * @param out the beginning of the destination range.
+         * @param endTolerance defines maximum allowed distance to end path point in addition to agentHalfExtents
          * @return Output iterator to the element in the destination range, one past the last element of found path.
          * Equal to out if no path is found.
          */
         template <class OutputIterator>
         Status findPath(const osg::Vec3f& agentHalfExtents, const float stepSize, const osg::Vec3f& start,
             const osg::Vec3f& end, const Flags includeFlags, const DetourNavigator::AreaCosts& areaCosts,
-            OutputIterator& out) const
+            float endTolerance, OutputIterator& out) const
         {
             static_assert(
                 std::is_same<
@@ -194,7 +195,7 @@ namespace DetourNavigator
             const auto settings = getSettings();
             return findSmoothPath(navMesh->lockConst()->getImpl(), toNavMeshCoordinates(settings, agentHalfExtents),
                 toNavMeshCoordinates(settings, stepSize), toNavMeshCoordinates(settings, start),
-                toNavMeshCoordinates(settings, end), includeFlags, areaCosts, settings, out);
+                toNavMeshCoordinates(settings, end), includeFlags, areaCosts, settings, endTolerance, out);
         }
 
         /**

components/detournavigator/status.hpp

@@ -6,6 +6,7 @@ namespace DetourNavigator
     enum class Status
     {
         Success,
+        PartialPath,
         NavMeshNotFound,
         StartPolygonNotFound,
         EndPolygonNotFound,
@@ -21,6 +22,8 @@ namespace DetourNavigator
         {
             case Status::Success:
                 return "success";
+            case Status::PartialPath:
+                return "partial path is found";
             case Status::NavMeshNotFound:
                 return "navmesh is not found";
             case Status::StartPolygonNotFound: