Merge branch 'aipursue_path' into 'master'

Make AiPursue path destination to be as close as possible to target (#6211)

Closes #6211

See merge request OpenMW/openmw!1154

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: