Merge branch 'fix_find_path' into 'master'

Remove stepping from findSmoothPath and other tweaks (#7241 , #7485)

Closes #7485 and #7241

See merge request OpenMW/openmw!3264

apps/openmw/mwlua/nearbybindings.cpp

@@ -163,21 +163,19 @@ namespace MWLua
                 { "MoveAlongSurfaceFailed", DetourNavigator::Status::MoveAlongSurfaceFailed },
                 { "FindPathOverPolygonsFailed", DetourNavigator::Status::FindPathOverPolygonsFailed },
                 { "InitNavMeshQueryFailed", DetourNavigator::Status::InitNavMeshQueryFailed },
+                { "FindStraightPathFailed", DetourNavigator::Status::FindStraightPathFailed },
             }));
 
         static const DetourNavigator::AgentBounds defaultAgentBounds{
             Settings::game().mActorCollisionShapeType,
             Settings::game().mDefaultActorPathfindHalfExtents,
         };
-        static const float defaultStepSize
-            = 2 * std::max(defaultAgentBounds.mHalfExtents.x(), defaultAgentBounds.mHalfExtents.y());
         static constexpr DetourNavigator::Flags defaultIncludeFlags = DetourNavigator::Flag_walk
             | DetourNavigator::Flag_swim | DetourNavigator::Flag_openDoor | DetourNavigator::Flag_usePathgrid;
 
         api["findPath"]
             = [](const osg::Vec3f& source, const osg::Vec3f& destination, const sol::optional<sol::table>& options) {
                   DetourNavigator::AgentBounds agentBounds = defaultAgentBounds;
-                  float stepSize = defaultStepSize;
                   DetourNavigator::Flags includeFlags = defaultIncludeFlags;
                   DetourNavigator::AreaCosts areaCosts{};
                   float destinationTolerance = 1;
@@ -189,13 +187,8 @@ namespace MWLua
                           if (const auto& v = t->get<sol::optional<DetourNavigator::CollisionShapeType>>("shapeType"))
                               agentBounds.mShapeType = *v;
                           if (const auto& v = t->get<sol::optional<osg::Vec3f>>("halfExtents"))
-                          {
                               agentBounds.mHalfExtents = *v;
-                              stepSize = 2 * std::max(v->x(), v->y());
-                          }
                       }
-                      if (const auto& v = options->get<sol::optional<float>>("stepSize"))
-                          stepSize = *v;
                       if (const auto& v = options->get<sol::optional<DetourNavigator::Flags>>("includeFlags"))
                           includeFlags = *v;
                       if (const auto& t = options->get<sol::optional<sol::table>>("areaCosts"))
@@ -216,8 +209,8 @@ namespace MWLua
                   std::vector<osg::Vec3f> result;
 
                   const DetourNavigator::Status status = DetourNavigator::findPath(
-                      *MWBase::Environment::get().getWorld()->getNavigator(), agentBounds, stepSize, source,
-                      destination, includeFlags, areaCosts, destinationTolerance, std::back_inserter(result));
+                      *MWBase::Environment::get().getWorld()->getNavigator(), agentBounds, source, destination,
+                      includeFlags, areaCosts, destinationTolerance, std::back_inserter(result));
 
                   return std::make_tuple(status, std::move(result));
               };

apps/openmw/mwmechanics/aipackage.cpp

@@ -478,11 +478,7 @@ DetourNavigator::Flags MWMechanics::AiPackage::getNavigatorFlags(const MWWorld::
         result |= DetourNavigator::Flag_swim;
 
     if (actorClass.canWalk(actor) && actor.getClass().getWalkSpeed(actor) > 0)
-    {
-        result |= DetourNavigator::Flag_walk;
-        if (getTypeId() == AiPackageTypeId::Travel)
-            result |= DetourNavigator::Flag_usePathgrid;
-    }
+        result |= DetourNavigator::Flag_walk | DetourNavigator::Flag_usePathgrid;
 
     if (canOpenDoors(actor) && getTypeId() != AiPackageTypeId::Wander)
         result |= DetourNavigator::Flag_openDoor;

apps/openmw/mwmechanics/pathfinding.cpp

@@ -439,10 +439,9 @@ namespace MWMechanics
         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 = DetourNavigator::findPath(
-            *navigator, agentBounds, stepSize, startPoint, endPoint, flags, areaCosts, endTolerance, out);
+            *navigator, agentBounds, startPoint, endPoint, flags, areaCosts, endTolerance, out);
 
         if (pathType == PathType::Partial && status == DetourNavigator::Status::PartialPath)
             return DetourNavigator::Status::Success;
@@ -475,8 +474,8 @@ namespace MWMechanics
         std::deque<osg::Vec3f> prePath;
         auto prePathInserter = std::back_inserter(prePath);
         const float endTolerance = 0;
-        const auto status = DetourNavigator::findPath(*navigator, agentBounds, stepSize, startPoint, mPath.front(),
-            flags, areaCosts, endTolerance, prePathInserter);
+        const auto status = DetourNavigator::findPath(
+            *navigator, agentBounds, startPoint, mPath.front(), flags, areaCosts, endTolerance, prePathInserter);
 
         if (status == DetourNavigator::Status::NavMeshNotFound)
             return;

apps/openmw_test_suite/detournavigator/navigator.cpp

@@ -50,7 +50,6 @@ namespace
         osg::Vec3f mEnd;
         std::deque<osg::Vec3f> mPath;
         std::back_insert_iterator<std::deque<osg::Vec3f>> mOut;
-        float mStepSize;
         AreaCosts mAreaCosts;
         Loading::Listener mListener;
         const osg::Vec2i mCellPosition{ 0, 0 };
@@ -65,7 +64,6 @@ namespace
             , mStart(52, 460, 1)
             , mEnd(460, 52, 1)
             , mOut(mPath)
-            , mStepSize(28.333332061767578125f)
         {
             mNavigator.reset(new NavigatorImpl(
                 mSettings, std::make_unique<NavMeshDb>(":memory:", std::numeric_limits<std::uint64_t>::max())));
@@ -129,8 +127,7 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty)
     {
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::NavMeshNotFound);
         EXPECT_EQ(mPath, std::deque<osg::Vec3f>());
     }
@@ -138,8 +135,7 @@ namespace
     TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception)
     {
         ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::StartPolygonNotFound);
     }
 
@@ -148,8 +144,7 @@ namespace
         ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
         ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
         mNavigator->removeAgent(mAgentBounds);
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::StartPolygonNotFound);
     }
 
@@ -172,33 +167,13 @@ namespace
         updateGuard.reset();
         mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375),
-                Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875),
-                Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125),
-                Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875),
-                Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875),
-                Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375),
-                Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125),
-                Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875),
-                Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375),
-                Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125),
-                Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625),
-                Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875),
-                Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125),
-                Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375),
-                Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375),
-                Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875),
-                Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625),
-                Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625),
-                Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125),
-                Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875),
-                Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 1.99999392032623291015625),
+                Vec3fEq(460, 56.66664886474609375, 1.99999392032623291015625)))
             << mPath;
     }
 
@@ -227,32 +202,12 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375),
-                Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875),
-                Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125),
-                Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875),
-                Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875),
-                Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375),
-                Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125),
-                Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875),
-                Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375),
-                Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125),
-                Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625),
-                Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875),
-                Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125),
-                Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375),
-                Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375),
-                Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875),
-                Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625),
-                Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625),
-                Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125),
-                Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875),
+            ElementsAre( //
+                Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
                 Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
             << mPath;
 
@@ -266,34 +221,15 @@ namespace
 
         mPath.clear();
         mOut = std::back_inserter(mPath);
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(69.5299530029296875, 434.754913330078125, -2.6775772571563720703125),
-                Vec3fEq(82.39324951171875, 409.50982666015625, -7.355137348175048828125),
-                Vec3fEq(95.25653839111328125, 384.2647705078125, -12.0326976776123046875),
-                Vec3fEq(108.11983489990234375, 359.019683837890625, -16.71025848388671875),
-                Vec3fEq(120.983123779296875, 333.774627685546875, -21.3878192901611328125),
-                Vec3fEq(133.8464202880859375, 308.529541015625, -26.0653781890869140625),
-                Vec3fEq(146.7097015380859375, 283.284454345703125, -30.7429370880126953125),
-                Vec3fEq(159.572998046875, 258.039398193359375, -35.420497894287109375),
-                Vec3fEq(172.4362945556640625, 232.7943115234375, -27.2731761932373046875),
-                Vec3fEq(185.2996063232421875, 207.54925537109375, -19.575878143310546875),
-                Vec3fEq(206.6449737548828125, 188.917236328125, -20.3511219024658203125),
-                Vec3fEq(227.9903564453125, 170.28521728515625, -22.9776935577392578125),
-                Vec3fEq(253.4362640380859375, 157.8239593505859375, -31.1692962646484375),
-                Vec3fEq(278.8822021484375, 145.3627166748046875, -30.253124237060546875),
-                Vec3fEq(304.328094482421875, 132.9014739990234375, -22.219127655029296875),
-                Vec3fEq(329.774017333984375, 120.44022369384765625, -13.2701435089111328125),
-                Vec3fEq(355.219940185546875, 107.97898101806640625, -5.330339908599853515625),
-                Vec3fEq(380.665863037109375, 95.51773834228515625, -3.5501649379730224609375),
-                Vec3fEq(406.111785888671875, 83.05649566650390625, -1.76998889446258544921875),
-                Vec3fEq(431.557708740234375, 70.5952606201171875, 0.01018683053553104400634765625),
-                Vec3fEq(457.003662109375, 58.134021759033203125, 1.79036080837249755859375),
-                Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 1.99999392032623291015625),
+                Vec3fEq(181.33331298828125, 215.33331298828125, -20.6666717529296875),
+                Vec3fEq(215.33331298828125, 181.33331298828125, -20.6666717529296875),
+                Vec3fEq(460, 56.66664886474609375, 1.99999392032623291015625)))
             << mPath;
     }
 
@@ -320,34 +256,15 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(69.5299530029296875, 434.754913330078125, -2.6775772571563720703125),
-                Vec3fEq(82.39324951171875, 409.50982666015625, -7.355137348175048828125),
-                Vec3fEq(95.25653839111328125, 384.2647705078125, -12.0326976776123046875),
-                Vec3fEq(108.11983489990234375, 359.019683837890625, -16.71025848388671875),
-                Vec3fEq(120.983123779296875, 333.774627685546875, -21.3878192901611328125),
-                Vec3fEq(133.8464202880859375, 308.529541015625, -26.0653781890869140625),
-                Vec3fEq(146.7097015380859375, 283.284454345703125, -30.7429370880126953125),
-                Vec3fEq(159.572998046875, 258.039398193359375, -35.420497894287109375),
-                Vec3fEq(172.4362945556640625, 232.7943115234375, -27.2731761932373046875),
-                Vec3fEq(185.2996063232421875, 207.54925537109375, -19.575878143310546875),
-                Vec3fEq(206.6449737548828125, 188.917236328125, -20.3511219024658203125),
-                Vec3fEq(227.9903564453125, 170.28521728515625, -22.9776935577392578125),
-                Vec3fEq(253.4362640380859375, 157.8239593505859375, -31.1692962646484375),
-                Vec3fEq(278.8822021484375, 145.3627166748046875, -30.253124237060546875),
-                Vec3fEq(304.328094482421875, 132.9014739990234375, -22.219127655029296875),
-                Vec3fEq(329.774017333984375, 120.44022369384765625, -13.2701435089111328125),
-                Vec3fEq(355.219940185546875, 107.97898101806640625, -5.330339908599853515625),
-                Vec3fEq(380.665863037109375, 95.51773834228515625, -3.5501649379730224609375),
-                Vec3fEq(406.111785888671875, 83.05649566650390625, -1.76998889446258544921875),
-                Vec3fEq(431.557708740234375, 70.5952606201171875, 0.01018683053553104400634765625),
-                Vec3fEq(457.003662109375, 58.134021759033203125, 1.79036080837249755859375),
-                Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 1.99999392032623291015625),
+                Vec3fEq(181.33331298828125, 215.33331298828125, -20.6666717529296875),
+                Vec3fEq(215.33331298828125, 181.33331298828125, -20.6666717529296875),
+                Vec3fEq(460, 56.66664886474609375, 1.99999392032623291015625)))
             << mPath;
 
         compound.shape().updateChildTransform(0, btTransform(btMatrix3x3::getIdentity(), btVector3(1000, 0, 0)));
@@ -359,33 +276,13 @@ namespace
 
         mPath.clear();
         mOut = std::back_inserter(mPath);
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375),
-                Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875),
-                Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125),
-                Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875),
-                Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875),
-                Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375),
-                Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125),
-                Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875),
-                Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375),
-                Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125),
-                Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625),
-                Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875),
-                Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125),
-                Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375),
-                Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375),
-                Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875),
-                Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625),
-                Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625),
-                Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125),
-                Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875),
-                Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 1.99999392032623291015625),
+                Vec3fEq(460, 56.66664886474609375, 1.99999392032623291015625)))
             << mPath;
     }
 
@@ -419,33 +316,13 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(76.70135498046875, 439.965301513671875, -0.903246104717254638671875),
-                Vec3fEq(96.73604583740234375, 419.93060302734375, -3.8064472675323486328125),
-                Vec3fEq(116.770751953125, 399.89593505859375, -6.709649562835693359375),
-                Vec3fEq(136.8054351806640625, 379.861236572265625, -9.33333873748779296875),
-                Vec3fEq(156.840118408203125, 359.826568603515625, -9.33333873748779296875),
-                Vec3fEq(176.8748016357421875, 339.7918701171875, -9.33333873748779296875),
-                Vec3fEq(196.90948486328125, 319.757171630859375, -9.33333873748779296875),
-                Vec3fEq(216.944183349609375, 299.722503662109375, -9.33333873748779296875),
-                Vec3fEq(236.9788665771484375, 279.68780517578125, -9.33333873748779296875),
-                Vec3fEq(257.0135498046875, 259.65313720703125, -9.33333873748779296875),
-                Vec3fEq(277.048248291015625, 239.618438720703125, -9.33333873748779296875),
-                Vec3fEq(297.082916259765625, 219.583740234375, -9.33333873748779296875),
-                Vec3fEq(317.11761474609375, 199.549041748046875, -9.33333873748779296875),
-                Vec3fEq(337.15228271484375, 179.5143585205078125, -9.33333873748779296875),
-                Vec3fEq(357.186981201171875, 159.47967529296875, -9.33333873748779296875),
-                Vec3fEq(377.221649169921875, 139.4449920654296875, -9.33333873748779296875),
-                Vec3fEq(397.25634765625, 119.41030120849609375, -6.891522884368896484375),
-                Vec3fEq(417.291046142578125, 99.3756103515625, -4.053897380828857421875),
-                Vec3fEq(437.325714111328125, 79.340911865234375, -1.21627247333526611328125),
-                Vec3fEq(457.360443115234375, 59.3062286376953125, 1.621352672576904296875),
-                Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 1.99999392032623291015625),
+                Vec3fEq(460, 56.66664886474609375, 1.99999392032623291015625)))
             << mPath;
     }
 
@@ -520,34 +397,15 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(69.013885498046875, 434.49853515625, -0.74384129047393798828125),
-                Vec3fEq(81.36110687255859375, 408.997100830078125, -3.4876689910888671875),
-                Vec3fEq(93.7083282470703125, 383.495635986328125, -6.2314929962158203125),
-                Vec3fEq(106.0555419921875, 357.99420166015625, -8.97531890869140625),
-                Vec3fEq(118.40276336669921875, 332.49273681640625, -11.7191448211669921875),
-                Vec3fEq(130.7499847412109375, 306.991302490234375, -14.4629726409912109375),
-                Vec3fEq(143.0972137451171875, 281.4898681640625, -17.206798553466796875),
-                Vec3fEq(155.4444122314453125, 255.9884033203125, -19.9506206512451171875),
-                Vec3fEq(167.7916412353515625, 230.4869537353515625, -19.91887664794921875),
-                Vec3fEq(189.053619384765625, 211.75982666015625, -20.1138629913330078125),
-                Vec3fEq(210.3155975341796875, 193.032684326171875, -20.3088512420654296875),
-                Vec3fEq(231.577606201171875, 174.3055419921875, -20.503841400146484375),
-                Vec3fEq(252.839599609375, 155.5784149169921875, -19.9803981781005859375),
-                Vec3fEq(278.407989501953125, 143.3704071044921875, -17.2675113677978515625),
-                Vec3fEq(303.976348876953125, 131.16241455078125, -14.55462360382080078125),
-                Vec3fEq(329.54473876953125, 118.9544219970703125, -11.84173583984375),
-                Vec3fEq(355.11309814453125, 106.74642181396484375, -9.12884807586669921875),
-                Vec3fEq(380.681488037109375, 94.538421630859375, -6.4159603118896484375),
-                Vec3fEq(406.249847412109375, 82.33042144775390625, -3.7030735015869140625),
-                Vec3fEq(431.8182373046875, 70.1224365234375, -0.990187108516693115234375),
-                Vec3fEq(457.38665771484375, 57.9144439697265625, 1.72269880771636962890625),
-                Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 1.99999392032623291015625),
+                Vec3fEq(158.6666412353515625, 249.3332977294921875, -20.6666717529296875),
+                Vec3fEq(249.3332977294921875, 158.6666412353515625, -20.6666717529296875),
+                Vec3fEq(460, 56.66664886474609375, 1.99999392032623291015625)))
             << mPath;
     }
 
@@ -574,22 +432,12 @@ namespace
         mEnd.x() = 256;
         mEnd.z() = 300;
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_swim, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_swim, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(256, 460, 185.33331298828125), Vec3fEq(256, 431.666656494140625, 185.33331298828125),
-                Vec3fEq(256, 403.33331298828125, 185.33331298828125), Vec3fEq(256, 375, 185.33331298828125),
-                Vec3fEq(256, 346.666656494140625, 185.33331298828125),
-                Vec3fEq(256, 318.33331298828125, 185.33331298828125), Vec3fEq(256, 290, 185.33331298828125),
-                Vec3fEq(256, 261.666656494140625, 185.33331298828125),
-                Vec3fEq(256, 233.3333282470703125, 185.33331298828125), Vec3fEq(256, 205, 185.33331298828125),
-                Vec3fEq(256, 176.6666717529296875, 185.33331298828125),
-                Vec3fEq(256, 148.3333282470703125, 185.33331298828125), Vec3fEq(256, 120, 185.33331298828125),
-                Vec3fEq(256, 91.6666717529296875, 185.33331298828125),
-                Vec3fEq(255.999969482421875, 63.33333587646484375, 185.33331298828125),
-                Vec3fEq(255.999969482421875, 56.66666412353515625, 185.33331298828125)))
+            ElementsAre( //
+                Vec3fEq(256, 460, 185.3333282470703125), Vec3fEq(256, 56.66664886474609375, 185.3333282470703125)))
             << mPath;
     }
 
@@ -616,22 +464,13 @@ namespace
         mStart.x() = 256;
         mEnd.x() = 256;
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts,
-                      mEndTolerance, mOut),
+        EXPECT_EQ(
+            findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(256, 460, -129.4098663330078125),
-                Vec3fEq(256, 431.666656494140625, -129.6970062255859375),
-                Vec3fEq(256, 403.33331298828125, -129.6970062255859375), Vec3fEq(256, 375, -129.4439239501953125),
-                Vec3fEq(256, 346.666656494140625, -129.02587890625),
-                Vec3fEq(256, 318.33331298828125, -128.6078338623046875), Vec3fEq(256, 290, -128.1021728515625),
-                Vec3fEq(256, 261.666656494140625, -126.46875), Vec3fEq(256, 233.3333282470703125, -119.4891357421875),
-                Vec3fEq(256, 205, -110.62021636962890625), Vec3fEq(256, 176.6666717529296875, -101.7512969970703125),
-                Vec3fEq(256, 148.3333282470703125, -92.88237762451171875), Vec3fEq(256, 120, -75.29378509521484375),
-                Vec3fEq(256, 91.6666717529296875, -55.201839447021484375),
-                Vec3fEq(256.000030517578125, 63.33333587646484375, -34.800380706787109375),
-                Vec3fEq(256.000030517578125, 56.66666412353515625, -30.00003814697265625)))
+            ElementsAre( //
+                Vec3fEq(256, 460, -129.4098663330078125), Vec3fEq(256, 56.66664886474609375, -30.0000133514404296875)))
             << mPath;
     }
 
@@ -659,22 +498,13 @@ namespace
         mStart.x() = 256;
         mEnd.x() = 256;
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts,
-                      mEndTolerance, mOut),
+        EXPECT_EQ(
+            findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(256, 460, -129.4098663330078125),
-                Vec3fEq(256, 431.666656494140625, -129.6970062255859375),
-                Vec3fEq(256, 403.33331298828125, -129.6970062255859375), Vec3fEq(256, 375, -129.4439239501953125),
-                Vec3fEq(256, 346.666656494140625, -129.02587890625),
-                Vec3fEq(256, 318.33331298828125, -128.6078338623046875), Vec3fEq(256, 290, -128.1021728515625),
-                Vec3fEq(256, 261.666656494140625, -126.46875), Vec3fEq(256, 233.3333282470703125, -119.4891357421875),
-                Vec3fEq(256, 205, -110.62021636962890625), Vec3fEq(256, 176.6666717529296875, -101.7512969970703125),
-                Vec3fEq(256, 148.3333282470703125, -92.88237762451171875), Vec3fEq(256, 120, -75.29378509521484375),
-                Vec3fEq(256, 91.6666717529296875, -55.201839447021484375),
-                Vec3fEq(256.000030517578125, 63.33333587646484375, -34.800380706787109375),
-                Vec3fEq(256.000030517578125, 56.66666412353515625, -30.00003814697265625)))
+            ElementsAre(
+                Vec3fEq(256, 460, -129.4098663330078125), Vec3fEq(256, 56.66664886474609375, -30.0000133514404296875)))
             << mPath;
     }
 
@@ -701,22 +531,12 @@ namespace
         mStart.x() = 256;
         mEnd.x() = 256;
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(256, 460, -129.4098663330078125),
-                Vec3fEq(256, 431.666656494140625, -129.6970062255859375),
-                Vec3fEq(256, 403.33331298828125, -129.6970062255859375), Vec3fEq(256, 375, -129.4439239501953125),
-                Vec3fEq(256, 346.666656494140625, -129.02587890625),
-                Vec3fEq(256, 318.33331298828125, -128.6078338623046875), Vec3fEq(256, 290, -128.1021728515625),
-                Vec3fEq(256, 261.666656494140625, -126.46875), Vec3fEq(256, 233.3333282470703125, -119.4891357421875),
-                Vec3fEq(256, 205, -110.62021636962890625), Vec3fEq(256, 176.6666717529296875, -101.7512969970703125),
-                Vec3fEq(256, 148.3333282470703125, -92.88237762451171875), Vec3fEq(256, 120, -75.29378509521484375),
-                Vec3fEq(256, 91.6666717529296875, -55.201839447021484375),
-                Vec3fEq(256.000030517578125, 63.33333587646484375, -34.800380706787109375),
-                Vec3fEq(256.000030517578125, 56.66666412353515625, -30.00003814697265625)))
+            ElementsAre( //
+                Vec3fEq(256, 460, -129.4098663330078125), Vec3fEq(256, 56.66664886474609375, -30.0000133514404296875)))
             << mPath;
     }
 
@@ -747,33 +567,13 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375),
-                Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875),
-                Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125),
-                Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875),
-                Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875),
-                Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375),
-                Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125),
-                Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875),
-                Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375),
-                Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125),
-                Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625),
-                Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875),
-                Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125),
-                Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375),
-                Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375),
-                Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875),
-                Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625),
-                Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625),
-                Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125),
-                Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875),
-                Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 1.99999392032623291015625),
+                Vec3fEq(460, 56.66664886474609375, 1.99999392032623291015625)))
             << mPath;
     }
 
@@ -802,33 +602,13 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375),
-                Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875),
-                Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125),
-                Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875),
-                Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875),
-                Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375),
-                Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125),
-                Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875),
-                Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375),
-                Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125),
-                Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625),
-                Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875),
-                Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125),
-                Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375),
-                Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375),
-                Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875),
-                Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625),
-                Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625),
-                Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125),
-                Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875),
-                Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 1.99999392032623291015625),
+                Vec3fEq(460, 56.66664886474609375, 1.99999392032623291015625)))
             << mPath;
     }
 
@@ -909,34 +689,15 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125),
-                Vec3fEq(69.5299530029296875, 434.754913330078125, -2.6775772571563720703125),
-                Vec3fEq(82.39324951171875, 409.50982666015625, -7.355137348175048828125),
-                Vec3fEq(95.25653839111328125, 384.2647705078125, -12.0326976776123046875),
-                Vec3fEq(108.11983489990234375, 359.019683837890625, -16.71025848388671875),
-                Vec3fEq(120.983123779296875, 333.774627685546875, -21.3878192901611328125),
-                Vec3fEq(133.8464202880859375, 308.529541015625, -26.0653781890869140625),
-                Vec3fEq(146.7097015380859375, 283.284454345703125, -30.7429370880126953125),
-                Vec3fEq(159.572998046875, 258.039398193359375, -35.420497894287109375),
-                Vec3fEq(172.4362945556640625, 232.7943115234375, -27.2731761932373046875),
-                Vec3fEq(185.2996063232421875, 207.54925537109375, -20.3612518310546875),
-                Vec3fEq(206.6449737548828125, 188.917236328125, -20.578319549560546875),
-                Vec3fEq(227.9903564453125, 170.28521728515625, -26.291717529296875),
-                Vec3fEq(253.4362640380859375, 157.8239593505859375, -34.784488677978515625),
-                Vec3fEq(278.8822021484375, 145.3627166748046875, -30.253124237060546875),
-                Vec3fEq(304.328094482421875, 132.9014739990234375, -25.72176361083984375),
-                Vec3fEq(329.774017333984375, 120.44022369384765625, -21.1904010772705078125),
-                Vec3fEq(355.219940185546875, 107.97898101806640625, -16.6590404510498046875),
-                Vec3fEq(380.665863037109375, 95.51773834228515625, -12.127681732177734375),
-                Vec3fEq(406.111785888671875, 83.05649566650390625, -7.5963191986083984375),
-                Vec3fEq(431.557708740234375, 70.5952606201171875, -3.0649592876434326171875),
-                Vec3fEq(457.003662109375, 58.134021759033203125, 1.4664003849029541015625),
-                Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 1.99999392032623291015625),
+                Vec3fEq(181.33331298828125, 215.33331298828125, -20.6666717529296875),
+                Vec3fEq(215.33331298828125, 181.33331298828125, -20.6666717529296875),
+                Vec3fEq(460, 56.66664886474609375, 1.99999392032623291015625)))
             << mPath;
     }
 
@@ -1065,33 +826,12 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, 101.99999237060546875),
-                Vec3fEq(76.70135498046875, 439.965301513671875, 101.99999237060546875),
-                Vec3fEq(96.73604583740234375, 419.93060302734375, 101.99999237060546875),
-                Vec3fEq(116.770751953125, 399.89593505859375, 101.99999237060546875),
-                Vec3fEq(136.8054351806640625, 379.861236572265625, 101.99999237060546875),
-                Vec3fEq(156.840118408203125, 359.826568603515625, 101.99999237060546875),
-                Vec3fEq(176.8748016357421875, 339.7918701171875, 101.99999237060546875),
-                Vec3fEq(196.90948486328125, 319.757171630859375, 101.99999237060546875),
-                Vec3fEq(216.944183349609375, 299.722503662109375, 101.99999237060546875),
-                Vec3fEq(236.9788665771484375, 279.68780517578125, 101.99999237060546875),
-                Vec3fEq(257.0135498046875, 259.65313720703125, 101.99999237060546875),
-                Vec3fEq(277.048248291015625, 239.618438720703125, 101.99999237060546875),
-                Vec3fEq(297.082916259765625, 219.583740234375, 101.99999237060546875),
-                Vec3fEq(317.11761474609375, 199.549041748046875, 101.99999237060546875),
-                Vec3fEq(337.15228271484375, 179.5143585205078125, 101.99999237060546875),
-                Vec3fEq(357.186981201171875, 159.47967529296875, 101.99999237060546875),
-                Vec3fEq(377.221649169921875, 139.4449920654296875, 101.99999237060546875),
-                Vec3fEq(397.25634765625, 119.41030120849609375, 101.99999237060546875),
-                Vec3fEq(417.291046142578125, 99.3756103515625, 101.99999237060546875),
-                Vec3fEq(437.325714111328125, 79.340911865234375, 101.99999237060546875),
-                Vec3fEq(457.360443115234375, 59.3062286376953125, 101.99999237060546875),
-                Vec3fEq(460, 56.66666412353515625, 101.99999237060546875)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, 102), Vec3fEq(460, 56.66664886474609375, 102)))
             << mPath;
     }
 
@@ -1118,21 +858,13 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
             Status::PartialPath);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66664886474609375, 460, -2.5371043682098388671875),
-                Vec3fEq(76.42063140869140625, 439.6884765625, -2.9134314060211181640625),
-                Vec3fEq(96.17461395263671875, 419.376953125, -4.50826549530029296875),
-                Vec3fEq(115.9285888671875, 399.0654296875, -6.1030979156494140625),
-                Vec3fEq(135.6825714111328125, 378.753936767578125, -7.697928905487060546875),
-                Vec3fEq(155.436553955078125, 358.442413330078125, -20.9574985504150390625),
-                Vec3fEq(175.190521240234375, 338.130889892578125, -35.907512664794921875),
-                Vec3fEq(194.9445037841796875, 317.8193359375, -50.85752105712890625),
-                Vec3fEq(214.698486328125, 297.5078125, -65.807525634765625),
-                Vec3fEq(222.0001068115234375, 290.000091552734375, -71.333465576171875)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, -2.5371119976043701171875),
+                Vec3fEq(222, 290, -71.33342742919921875)))
             << mPath;
     }
 
@@ -1161,29 +893,13 @@ namespace
 
         const float endTolerance = 1000.0f;
 
-        EXPECT_EQ(
-            findPath(*mNavigator, mAgentBounds, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, endTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, endTolerance, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
-            ElementsAre(Vec3fEq(56.66666412353515625, 460, -2.5371043682098388671875),
-                Vec3fEq(71.5649566650390625, 435.899810791015625, -5.817593097686767578125),
-                Vec3fEq(86.46324920654296875, 411.79962158203125, -9.66499996185302734375),
-                Vec3fEq(101.36154937744140625, 387.699462890625, -13.512401580810546875),
-                Vec3fEq(116.2598419189453125, 363.599273681640625, -17.359806060791015625),
-                Vec3fEq(131.1581268310546875, 339.499114990234375, -21.2072086334228515625),
-                Vec3fEq(146.056427001953125, 315.39892578125, -25.0546112060546875),
-                Vec3fEq(160.9547271728515625, 291.298736572265625, -28.9020137786865234375),
-                Vec3fEq(175.8530120849609375, 267.198577880859375, -32.749416351318359375),
-                Vec3fEq(190.751312255859375, 243.098388671875, -33.819454193115234375),
-                Vec3fEq(205.64959716796875, 218.9982147216796875, -31.020172119140625),
-                Vec3fEq(220.5478973388671875, 194.898040771484375, -26.844608306884765625),
-                Vec3fEq(235.446197509765625, 170.7978668212890625, -26.785541534423828125),
-                Vec3fEq(250.3444671630859375, 146.6976776123046875, -26.7264766693115234375),
-                Vec3fEq(265.242767333984375, 122.59751129150390625, -20.59339141845703125),
-                Vec3fEq(280.141021728515625, 98.4973297119140625, -14.040531158447265625),
-                Vec3fEq(295.039306640625, 74.39715576171875, -7.48766994476318359375),
-                Vec3fEq(306, 56.66666412353515625, -2.6667339801788330078125)))
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 460, -2.5371119976043701171875),
+                Vec3fEq(305.999969482421875, 56.66664886474609375, -2.6667406558990478515625)))
             << mPath;
     }
 

components/detournavigator/asyncnavmeshupdater.cpp

@@ -339,7 +339,7 @@ namespace DetourNavigator
                     switch (status)
                     {
                         case JobStatus::Done:
-                            unlockTile(job->mAgentBounds, job->mChangedTile);
+                            unlockTile(job->mId, job->mAgentBounds, job->mChangedTile);
                             if (job->mGeneratedNavMeshData != nullptr)
                                 mDbWorker->enqueueJob(job);
                             else
@@ -565,12 +565,14 @@ namespace DetourNavigator
 
         mWaiting.pop_front();
 
+        Log(Debug::Debug) << "Pop job " << job->mId << " by thread=" << std::this_thread::get_id();
+
         if (job->mRecastMesh != nullptr)
             return job;
 
-        if (!lockTile(job->mAgentBounds, job->mChangedTile))
+        if (!lockTile(job->mId, job->mAgentBounds, job->mChangedTile))
         {
-            Log(Debug::Debug) << "Failed to lock tile by " << job->mId;
+            Log(Debug::Debug) << "Failed to lock tile by job " << job->mId << " try=" << job->mTryNumber;
             ++job->mTryNumber;
             insertPrioritizedJob(job, mWaiting);
             return mJobs.end();
@@ -610,7 +612,7 @@ namespace DetourNavigator
 
     void AsyncNavMeshUpdater::repost(JobIt job)
     {
-        unlockTile(job->mAgentBounds, job->mChangedTile);
+        unlockTile(job->mId, job->mAgentBounds, job->mChangedTile);
 
         if (mShouldStop || job->mTryNumber > 2)
             return;
@@ -628,17 +630,21 @@ namespace DetourNavigator
         mJobs.erase(job);
     }
 
-    bool AsyncNavMeshUpdater::lockTile(const AgentBounds& agentBounds, const TilePosition& changedTile)
+    bool AsyncNavMeshUpdater::lockTile(
+        std::size_t jobId, const AgentBounds& agentBounds, const TilePosition& changedTile)
     {
-        Log(Debug::Debug) << "Locking tile agent=" << agentBounds << " changedTile=(" << changedTile << ")";
+        Log(Debug::Debug) << "Locking tile by job " << jobId << " agent=" << agentBounds << " changedTile=("
+                          << changedTile << ")";
         return mProcessingTiles.lock()->emplace(agentBounds, changedTile).second;
     }
 
-    void AsyncNavMeshUpdater::unlockTile(const AgentBounds& agentBounds, const TilePosition& changedTile)
+    void AsyncNavMeshUpdater::unlockTile(
+        std::size_t jobId, const AgentBounds& agentBounds, const TilePosition& changedTile)
     {
         auto locked = mProcessingTiles.lock();
         locked->erase(std::tie(agentBounds, changedTile));
-        Log(Debug::Debug) << "Unlocked tile agent=" << agentBounds << " changedTile=(" << changedTile << ")";
+        Log(Debug::Debug) << "Unlocked tile by job " << jobId << " agent=" << agentBounds << " changedTile=("
+                          << changedTile << ")";
         if (locked->empty())
             mProcessed.notify_all();
     }

components/detournavigator/asyncnavmeshupdater.hpp

@@ -199,9 +199,9 @@ namespace DetourNavigator
 
         void repost(JobIt job);
 
-        bool lockTile(const AgentBounds& agentBounds, const TilePosition& changedTile);
+        bool lockTile(std::size_t jobId, const AgentBounds& agentBounds, const TilePosition& changedTile);
 
-        void unlockTile(const AgentBounds& agentBounds, const TilePosition& changedTile);
+        void unlockTile(std::size_t jobId, const AgentBounds& agentBounds, const TilePosition& changedTile);
 
         inline std::size_t getTotalJobs() const;
 

components/detournavigator/debug.cpp

@@ -36,9 +36,11 @@ namespace DetourNavigator
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(NavMeshNotFound)
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(StartPolygonNotFound)
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(EndPolygonNotFound)
+            OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(TargetPolygonNotFound)
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(MoveAlongSurfaceFailed)
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(FindPathOverPolygonsFailed)
             OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(InitNavMeshQueryFailed)
+            OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(FindStraightPathFailed)
         }
 #undef OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE
         return stream << "DetourNavigator::Error::" << static_cast<int>(value);

components/detournavigator/findrandompointaroundcircle.cpp

@@ -1,5 +1,4 @@
 #include "findrandompointaroundcircle.hpp"
-#include "findsmoothpath.hpp"
 
 #include <DetourNavMesh.h>
 #include <DetourNavMeshQuery.h>
@@ -13,9 +12,11 @@ namespace DetourNavigator
         dtQueryFilter queryFilter;
         queryFilter.setIncludeFlags(includeFlags);
 
-        dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, halfExtents * 4);
-        if (startRef == 0)
-            return std::optional<osg::Vec3f>();
+        dtPolyRef startRef = 0;
+        const dtStatus status
+            = navMeshQuery.findNearestPoly(start.ptr(), halfExtents.ptr(), &queryFilter, &startRef, nullptr);
+        if (dtStatusFailed(status))
+            return std::nullopt;
 
         dtPolyRef resultRef = 0;
         osg::Vec3f resultPosition;

components/detournavigator/findsmoothpath.cpp

@@ -1,147 +0,0 @@
-#include "findsmoothpath.hpp"
-
-#include <components/misc/convert.hpp>
-
-#include <DetourCommon.h>
-
-#include <algorithm>
-#include <array>
-
-namespace DetourNavigator
-{
-    std::size_t fixupCorridor(std::vector<dtPolyRef>& path, std::size_t pathSize, const std::vector<dtPolyRef>& visited)
-    {
-        std::vector<dtPolyRef>::const_reverse_iterator furthestVisited;
-
-        // Find furthest common polygon.
-        const auto begin = path.begin();
-        const auto end = path.begin() + pathSize;
-        const std::reverse_iterator rbegin(end);
-        const std::reverse_iterator rend(begin);
-        const auto it = std::find_if(rbegin, rend, [&](dtPolyRef pathValue) {
-            const auto it = std::find(visited.rbegin(), visited.rend(), pathValue);
-            if (it == visited.rend())
-                return false;
-            furthestVisited = it;
-            return true;
-        });
-
-        // If no intersection found just return current path.
-        if (it == rend)
-            return pathSize;
-        const auto furthestPath = it.base() - 1;
-
-        // Concatenate paths.
-
-        // visited: a_1 ... a_n x b_1 ... b_n
-        //      furthestVisited ^
-        //    path: C x D            E
-        //            ^ furthestPath ^ path.size() - (furthestVisited + 1 - visited.rbegin())
-        //  result: x b_n ... b_1 D
-
-        const std::size_t required = static_cast<std::size_t>(furthestVisited + 1 - visited.rbegin());
-        const auto newEnd = std::copy(furthestPath + 1, std::min(begin + path.size(), end), begin + required);
-        std::copy(visited.rbegin(), furthestVisited + 1, begin);
-
-        return static_cast<std::size_t>(newEnd - begin);
-    }
-
-    std::size_t fixupShortcuts(dtPolyRef* path, std::size_t pathSize, const dtNavMeshQuery& navQuery)
-    {
-        if (pathSize < 3)
-            return pathSize;
-
-        // Get connected polygons
-        const dtMeshTile* tile = nullptr;
-        const dtPoly* poly = nullptr;
-        if (dtStatusFailed(navQuery.getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly)))
-            return pathSize;
-
-        const std::size_t maxNeis = 16;
-        std::array<dtPolyRef, maxNeis> neis;
-        std::size_t nneis = 0;
-
-        for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
-        {
-            const dtLink* link = &tile->links[k];
-            if (link->ref != 0)
-            {
-                if (nneis < maxNeis)
-                    neis[nneis++] = link->ref;
-            }
-        }
-
-        // If any of the neighbour polygons is within the next few polygons
-        // in the path, short cut to that polygon directly.
-        const std::size_t maxLookAhead = 6;
-        std::size_t cut = 0;
-        for (std::size_t i = std::min(maxLookAhead, pathSize) - 1; i > 1 && cut == 0; i--)
-        {
-            for (std::size_t j = 0; j < nneis; j++)
-            {
-                if (path[i] == neis[j])
-                {
-                    cut = i;
-                    break;
-                }
-            }
-        }
-        if (cut <= 1)
-            return pathSize;
-
-        const std::ptrdiff_t offset = static_cast<std::ptrdiff_t>(cut) - 1;
-        std::copy(path + offset, path + pathSize, path);
-        return pathSize - offset;
-    }
-
-    std::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& startPos,
-        const osg::Vec3f& endPos, const float minTargetDist, const dtPolyRef* path, const std::size_t pathSize)
-    {
-        // Find steer target.
-        SteerTarget result;
-        constexpr int maxSteerPoints = 3;
-        std::array<float, maxSteerPoints * 3> steerPath;
-        std::array<unsigned char, maxSteerPoints> steerPathFlags;
-        std::array<dtPolyRef, maxSteerPoints> steerPathPolys;
-        int nsteerPath = 0;
-        const dtStatus status
-            = navMeshQuery.findStraightPath(startPos.ptr(), endPos.ptr(), path, static_cast<int>(pathSize),
-                steerPath.data(), steerPathFlags.data(), steerPathPolys.data(), &nsteerPath, maxSteerPoints);
-        if (dtStatusFailed(status))
-            return std::nullopt;
-        assert(nsteerPath >= 0);
-        if (!nsteerPath)
-            return std::nullopt;
-
-        // Find vertex far enough to steer to.
-        std::size_t ns = 0;
-        while (ns < static_cast<std::size_t>(nsteerPath))
-        {
-            // Stop at Off-Mesh link or when point is further than slop away.
-            if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
-                || !inRange(Misc::Convert::makeOsgVec3f(&steerPath[ns * 3]), startPos, minTargetDist))
-                break;
-            ns++;
-        }
-        // Failed to find good point to steer to.
-        if (ns >= static_cast<std::size_t>(nsteerPath))
-            return std::nullopt;
-
-        dtVcopy(result.mSteerPos.ptr(), &steerPath[ns * 3]);
-        result.mSteerPos.y() = startPos[1];
-        result.mSteerPosFlag = steerPathFlags[ns];
-        result.mSteerPosRef = steerPathPolys[ns];
-
-        return result;
-    }
-
-    dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter, const osg::Vec3f& center,
-        const osg::Vec3f& halfExtents)
-    {
-        dtPolyRef ref = 0;
-        const dtStatus status = query.findNearestPoly(center.ptr(), halfExtents.ptr(), &filter, &ref, nullptr);
-        if (!dtStatusSucceed(status))
-            return 0;
-        return ref;
-    }
-}

components/detournavigator/findsmoothpath.hpp

@@ -7,59 +7,27 @@
 #include "settingsutils.hpp"
 #include "status.hpp"
 
+#include <DetourCommon.h>
 #include <DetourNavMesh.h>
 #include <DetourNavMeshQuery.h>
 
 #include <osg/Vec3f>
 
+#include <array>
 #include <cassert>
 #include <functional>
+#include <span>
 #include <vector>
 
-class dtNavMesh;
-
 namespace DetourNavigator
 {
-    struct Settings;
-
-    inline bool inRange(const osg::Vec3f& v1, const osg::Vec3f& v2, const float r)
-    {
-        return (osg::Vec2f(v1.x(), v1.z()) - osg::Vec2f(v2.x(), v2.z())).length() < r;
-    }
-
-    std::size_t fixupCorridor(
-        std::vector<dtPolyRef>& path, std::size_t pathSize, const std::vector<dtPolyRef>& visited);
-
-    // This function checks if the path has a small U-turn, that is,
-    // a polygon further in the path is adjacent to the first polygon
-    // in the path. If that happens, a shortcut is taken.
-    // This can happen if the target (T) location is at tile boundary,
-    // and we're (S) approaching it parallel to the tile edge.
-    // The choice at the vertex can be arbitrary,
-    //  +---+---+
-    //  |:::|:::|
-    //  +-S-+-T-+
-    //  |:::|   | <-- the step can end up in here, resulting U-turn path.
-    //  +---+---+
-    std::size_t fixupShortcuts(dtPolyRef* path, std::size_t pathSize, const dtNavMeshQuery& navQuery);
-
-    struct SteerTarget
-    {
-        osg::Vec3f mSteerPos;
-        unsigned char mSteerPosFlag;
-        dtPolyRef mSteerPosRef;
-    };
-
-    std::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos,
-        const osg::Vec3f& endPos, const float minTargetDist, const dtPolyRef* path, const std::size_t pathSize);
-
-    template <class OutputIterator>
+    template <class OutputIterator, class Function>
     class OutputTransformIterator
     {
     public:
-        explicit OutputTransformIterator(OutputIterator& impl, const RecastSettings& settings)
+        explicit OutputTransformIterator(OutputIterator& impl, Function&& function)
             : mImpl(impl)
-            , mSettings(settings)
+            , mFunction(std::forward<Function>(function))
         {
         }
 
@@ -80,180 +48,64 @@ namespace DetourNavigator
 
         OutputTransformIterator& operator=(const osg::Vec3f& value)
         {
-            *mImpl.get() = fromNavMeshCoordinates(mSettings, value);
+            *mImpl.get() = mFunction(value);
             return *this;
         }
 
     private:
         std::reference_wrapper<OutputIterator> mImpl;
-        std::reference_wrapper<const RecastSettings> mSettings;
+        Function mFunction;
     };
 
-    inline bool initNavMeshQuery(dtNavMeshQuery& value, const dtNavMesh& navMesh, const int maxNodes)
-    {
-        const auto status = value.init(&navMesh, maxNodes);
-        return dtStatusSucceed(status);
-    }
-
-    dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter, const osg::Vec3f& center,
-        const osg::Vec3f& halfExtents);
-
-    struct MoveAlongSurfaceResult
-    {
-        osg::Vec3f mResultPos;
-        std::vector<dtPolyRef> mVisited;
-    };
-
-    inline std::optional<MoveAlongSurfaceResult> moveAlongSurface(const dtNavMeshQuery& navMeshQuery,
-        const dtPolyRef startRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& filter,
-        const std::size_t maxVisitedSize)
+    template <class OutputIterator>
+    auto withFromNavMeshCoordinates(OutputIterator& impl, const RecastSettings& settings)
     {
-        MoveAlongSurfaceResult result;
-        result.mVisited.resize(maxVisitedSize);
-        int visitedNumber = 0;
-        const auto status = navMeshQuery.moveAlongSurface(startRef, startPos.ptr(), endPos.ptr(), &filter,
-            result.mResultPos.ptr(), result.mVisited.data(), &visitedNumber, static_cast<int>(maxVisitedSize));
-        if (!dtStatusSucceed(status))
-            return {};
-        assert(visitedNumber >= 0);
-        assert(visitedNumber <= static_cast<int>(maxVisitedSize));
-        result.mVisited.resize(static_cast<std::size_t>(visitedNumber));
-        return { std::move(result) };
+        return OutputTransformIterator(
+            impl, [&settings](const osg::Vec3f& value) { return fromNavMeshCoordinates(settings, value); });
     }
 
     inline std::optional<std::size_t> findPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef,
         const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter,
-        dtPolyRef* path, const std::size_t maxSize)
+        std::span<dtPolyRef> pathBuffer)
     {
         int pathLen = 0;
-        const auto status = navMeshQuery.findPath(
-            startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter, path, &pathLen, static_cast<int>(maxSize));
+        const auto status = navMeshQuery.findPath(startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter,
+            pathBuffer.data(), &pathLen, static_cast<int>(pathBuffer.size()));
         if (!dtStatusSucceed(status))
             return {};
         assert(pathLen >= 0);
-        assert(static_cast<std::size_t>(pathLen) <= maxSize);
+        assert(static_cast<std::size_t>(pathLen) <= pathBuffer.size());
         return static_cast<std::size_t>(pathLen);
     }
 
     template <class OutputIterator>
-    Status makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, const dtQueryFilter& filter,
-        const osg::Vec3f& start, const osg::Vec3f& end, const float stepSize, std::vector<dtPolyRef>& polygonPath,
-        std::size_t polygonPathSize, std::size_t maxSmoothPathSize, OutputIterator& out)
-    {
-        // Iterate over the path to find smooth path on the detail mesh surface.
-        osg::Vec3f iterPos;
-        navMeshQuery.closestPointOnPoly(polygonPath.front(), start.ptr(), iterPos.ptr(), nullptr);
-
-        osg::Vec3f targetPos;
-        navMeshQuery.closestPointOnPoly(polygonPath[polygonPathSize - 1], end.ptr(), targetPos.ptr(), nullptr);
-
-        constexpr float slop = 0.01f;
-
-        *out++ = iterPos;
-
-        std::size_t smoothPathSize = 1;
-
-        // Move towards target a small advancement at a time until target reached or
-        // when ran out of memory to store the path.
-        while (polygonPathSize > 0 && smoothPathSize < maxSmoothPathSize)
-        {
-            // Find location to steer towards.
-            const auto steerTarget
-                = getSteerTarget(navMeshQuery, iterPos, targetPos, slop, polygonPath.data(), polygonPathSize);
-
-            if (!steerTarget)
-                break;
-
-            const bool endOfPath = bool(steerTarget->mSteerPosFlag & DT_STRAIGHTPATH_END);
-            const bool offMeshConnection = bool(steerTarget->mSteerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
-
-            // Find movement delta.
-            const osg::Vec3f delta = steerTarget->mSteerPos - iterPos;
-            float len = delta.length();
-            // If the steer target is end of path or off-mesh link, do not move past the location.
-            if ((endOfPath || offMeshConnection) && len < stepSize)
-                len = 1;
-            else
-                len = stepSize / len;
-
-            const osg::Vec3f moveTgt = iterPos + delta * len;
-            const auto result = moveAlongSurface(navMeshQuery, polygonPath.front(), iterPos, moveTgt, filter, 16);
-
-            if (!result)
-                return Status::MoveAlongSurfaceFailed;
-
-            polygonPathSize = fixupCorridor(polygonPath, polygonPathSize, result->mVisited);
-            polygonPathSize = fixupShortcuts(polygonPath.data(), polygonPathSize, navMeshQuery);
-
-            // Handle end of path and off-mesh links when close enough.
-            if (endOfPath && inRange(result->mResultPos, steerTarget->mSteerPos, slop))
-            {
-                // Reached end of path.
-                iterPos = targetPos;
-                *out++ = iterPos;
-                ++smoothPathSize;
-                break;
-            }
-
-            dtPolyRef polyRef = polygonPath.front();
-            osg::Vec3f polyPos = result->mResultPos;
-
-            if (offMeshConnection && inRange(polyPos, steerTarget->mSteerPos, slop))
-            {
-                // Advance the path up to and over the off-mesh connection.
-                dtPolyRef prevRef = 0;
-                std::size_t npos = 0;
-                while (npos < polygonPathSize && polyRef != steerTarget->mSteerPosRef)
-                {
-                    prevRef = polyRef;
-                    polyRef = polygonPath[npos];
-                    ++npos;
-                }
-                if (npos > 0)
-                {
-                    std::copy(polygonPath.begin() + npos, polygonPath.begin() + polygonPathSize, polygonPath.begin());
-                    polygonPathSize -= npos;
-                }
-
-                // Reached off-mesh connection.
-                osg::Vec3f startPos;
-                osg::Vec3f endPos;
-
-                // Handle the connection.
-                if (dtStatusSucceed(
-                        navMesh.getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos.ptr(), endPos.ptr())))
-                {
-                    *out++ = startPos;
-                    ++smoothPathSize;
-
-                    // Hack to make the dotted path not visible during off-mesh connection.
-                    if (smoothPathSize & 1)
-                    {
-                        *out++ = startPos;
-                        ++smoothPathSize;
-                    }
-
-                    // Move position at the other side of the off-mesh link.
-                    polyPos = endPos;
-                }
-            }
-
-            navMeshQuery.getPolyHeight(polyRef, polyPos.ptr(), &iterPos.y());
-            iterPos.x() = result->mResultPos.x();
-            iterPos.z() = result->mResultPos.z();
-
-            // Store results.
-            *out++ = iterPos;
-            ++smoothPathSize;
-        }
+    Status makeSmoothPath(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& start, const osg::Vec3f& end,
+        std::span<dtPolyRef> polygonPath, std::size_t polygonPathSize, std::size_t maxSmoothPathSize,
+        OutputIterator& out)
+    {
+        assert(polygonPathSize <= polygonPath.size());
+
+        std::vector<float> cornerVertsBuffer(maxSmoothPathSize * 3);
+        std::vector<unsigned char> cornerFlagsBuffer(maxSmoothPathSize);
+        std::vector<dtPolyRef> cornerPolysBuffer(maxSmoothPathSize);
+        int cornersCount = 0;
+        constexpr int findStraightPathOptions = DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS;
+        if (const dtStatus status = navMeshQuery.findStraightPath(start.ptr(), end.ptr(), polygonPath.data(),
+                static_cast<int>(polygonPathSize), cornerVertsBuffer.data(), cornerFlagsBuffer.data(),
+                cornerPolysBuffer.data(), &cornersCount, static_cast<int>(maxSmoothPathSize), findStraightPathOptions);
+            dtStatusFailed(status))
+            return Status::FindStraightPathFailed;
+
+        for (int i = 0; i < cornersCount; ++i)
+            *out++ = Misc::Convert::makeOsgVec3f(&cornerVertsBuffer[static_cast<std::size_t>(i) * 3]);
 
         return Status::Success;
     }
 
     template <class OutputIterator>
-    Status findSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& halfExtents,
-        const float stepSize, const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags,
-        const AreaCosts& areaCosts, const Settings& settings, float endTolerance, OutputIterator out)
+    Status findSmoothPath(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& halfExtents, const osg::Vec3f& start,
+        const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts, const DetourSettings& settings,
+        float endTolerance, OutputIterator out)
     {
         dtQueryFilter queryFilter;
         queryFilter.setIncludeFlags(includeFlags);
@@ -265,18 +117,24 @@ namespace DetourNavigator
         constexpr float polyDistanceFactor = 4;
         const osg::Vec3f polyHalfExtents = halfExtents * polyDistanceFactor;
 
-        const dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, polyHalfExtents);
-        if (startRef == 0)
+        osg::Vec3f startNavMeshPos;
+        dtPolyRef startRef = 0;
+        if (const dtStatus status = navMeshQuery.findNearestPoly(
+                start.ptr(), polyHalfExtents.ptr(), &queryFilter, &startRef, startNavMeshPos.ptr());
+            dtStatusFailed(status) || startRef == 0)
             return Status::StartPolygonNotFound;
 
-        const dtPolyRef endRef = findNearestPoly(
-            navMeshQuery, queryFilter, end, polyHalfExtents + osg::Vec3f(endTolerance, endTolerance, endTolerance));
-        if (endRef == 0)
+        osg::Vec3f endNavMeshPos;
+        const osg::Vec3f endPolyHalfExtents = polyHalfExtents + osg::Vec3f(endTolerance, endTolerance, endTolerance);
+        dtPolyRef endRef;
+        if (const dtStatus status = navMeshQuery.findNearestPoly(
+                end.ptr(), endPolyHalfExtents.ptr(), &queryFilter, &endRef, endNavMeshPos.ptr());
+            dtStatusFailed(status) || endRef == 0)
             return Status::EndPolygonNotFound;
 
-        std::vector<dtPolyRef> polygonPath(settings.mDetour.mMaxPolygonPathSize);
+        std::vector<dtPolyRef> polygonPath(settings.mMaxPolygonPathSize);
         const auto polygonPathSize
-            = findPath(navMeshQuery, startRef, endRef, start, end, queryFilter, polygonPath.data(), polygonPath.size());
+            = findPath(navMeshQuery, startRef, endRef, startNavMeshPos, endNavMeshPos, queryFilter, polygonPath);
 
         if (!polygonPathSize.has_value())
             return Status::FindPathOverPolygonsFailed;
@@ -284,10 +142,15 @@ namespace DetourNavigator
         if (*polygonPathSize == 0)
             return Status::Success;
 
+        osg::Vec3f targetNavMeshPos;
+        if (const dtStatus status = navMeshQuery.closestPointOnPoly(
+                polygonPath[*polygonPathSize - 1], end.ptr(), targetNavMeshPos.ptr(), nullptr);
+            dtStatusFailed(status))
+            return Status::TargetPolygonNotFound;
+
         const bool partialPath = polygonPath[*polygonPathSize - 1] != endRef;
-        auto outTransform = OutputTransformIterator<OutputIterator>(out, settings.mRecast);
-        const Status smoothStatus = makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize,
-            polygonPath, *polygonPathSize, settings.mDetour.mMaxSmoothPathSize, outTransform);
+        const Status smoothStatus = makeSmoothPath(navMeshQuery, startNavMeshPos, targetNavMeshPos, polygonPath,
+            *polygonPathSize, settings.mMaxSmoothPathSize, out);
 
         if (smoothStatus != Status::Success)
             return smoothStatus;

components/detournavigator/navigatorutils.hpp

@@ -25,9 +25,9 @@ namespace DetourNavigator
      * Equal to out if no path is found.
      */
     template <class OutputIterator>
-    inline Status findPath(const Navigator& navigator, const AgentBounds& agentBounds, const float stepSize,
-        const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts,
-        float endTolerance, OutputIterator out)
+    inline Status findPath(const Navigator& navigator, const AgentBounds& agentBounds, const osg::Vec3f& start,
+        const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts, float endTolerance,
+        OutputIterator out)
     {
         static_assert(std::is_same<typename std::iterator_traits<OutputIterator>::iterator_category,
                           std::output_iterator_tag>::value,
@@ -36,11 +36,11 @@ namespace DetourNavigator
         if (navMesh == nullptr)
             return Status::NavMeshNotFound;
         const Settings& settings = navigator.getSettings();
+        auto outTransform = withFromNavMeshCoordinates(out, settings.mRecast);
         const auto locked = navMesh->lock();
-        return findSmoothPath(locked->getImpl(), locked->getQuery(),
-            toNavMeshCoordinates(settings.mRecast, agentBounds.mHalfExtents),
-            toNavMeshCoordinates(settings.mRecast, stepSize), toNavMeshCoordinates(settings.mRecast, start),
-            toNavMeshCoordinates(settings.mRecast, end), includeFlags, areaCosts, settings, endTolerance, out);
+        return findSmoothPath(locked->getQuery(), toNavMeshCoordinates(settings.mRecast, agentBounds.mHalfExtents),
+            toNavMeshCoordinates(settings.mRecast, start), toNavMeshCoordinates(settings.mRecast, end), includeFlags,
+            areaCosts, settings.mDetour, endTolerance, outTransform);
     }
 
     /**

components/detournavigator/status.hpp

@@ -10,9 +10,11 @@ namespace DetourNavigator
         NavMeshNotFound,
         StartPolygonNotFound,
         EndPolygonNotFound,
+        TargetPolygonNotFound,
         MoveAlongSurfaceFailed,
         FindPathOverPolygonsFailed,
         InitNavMeshQueryFailed,
+        FindStraightPathFailed,
     };
 
     constexpr const char* getMessage(Status value)
@@ -29,12 +31,16 @@ namespace DetourNavigator
                 return "polygon for start position is not found on navmesh";
             case Status::EndPolygonNotFound:
                 return "polygon for end position is not found on navmesh";
+            case Status::TargetPolygonNotFound:
+                return "polygon for target position is not found on navmesh";
             case Status::MoveAlongSurfaceFailed:
                 return "move along surface on navmesh is failed";
             case Status::FindPathOverPolygonsFailed:
                 return "path over navmesh polygons is not found";
             case Status::InitNavMeshQueryFailed:
                 return "failed to init navmesh query";
+            case Status::FindStraightPathFailed:
+                return "failed to straight path using polygon path";
         }
         return "unknown error";
     }

files/lua_api/openmw/nearby.lua

@@ -135,12 +135,16 @@
 -- @field [parent=#FIND_PATH_STATUS] #number EndPolygonNotFound `destination` position is too far from available
 -- navigation mesh. The status may appear when navigation mesh is not fully generated or position is outside of covered
 -- area;
+-- @field [parent=#FIND_PATH_STATUS] #number TargetPolygonNotFound adjusted `destination` position is too far from available
+-- navigation mesh. The status may appear when navigation mesh is not fully generated or position is outside of covered
+-- area;
 -- @field [parent=#FIND_PATH_STATUS] #number MoveAlongSurfaceFailed Found path couldn't be smoothed due to imperfect
 -- algorithm implementation or bad navigation mesh data;
 -- @field [parent=#FIND_PATH_STATUS] #number FindPathOverPolygonsFailed Path over navigation mesh from `source` to
 -- `destination` does not exist or navigation mesh is not fully generated to provide the path;
 -- @field [parent=#FIND_PATH_STATUS] #number InitNavMeshQueryFailed Couldn't initialize required data due to bad input
 -- or bad navigation mesh data.
+-- @field [parent=#FIND_PATH_STATUS] #number FindStraightPathFailed Couldn't map path over polygons into world coordinates.
 
 ---
 -- Find path over navigation mesh from source to destination with given options. Result is unstable since navigation
@@ -154,8 +158,6 @@
 --
 --     * `shapeType` - one of @{#COLLISION_SHAPE_TYPE} values;
 --     * `halfExtents` - @{openmw.util#Vector3} defining agent bounds size;
---   * `stepSize` - a floating point number to define frequency of path points
---     (default: `2 * math.max(halfExtents:x, halfExtents:y)`)
 --   * `includeFlags` - allowed areas for agent to move, a sum of @{#NAVIGATOR_FLAGS} values
 --     (default: @{#NAVIGATOR_FLAGS.Walk} + @{#NAVIGATOR_FLAGS.Swim} +
 --     @{#NAVIGATOR_FLAGS.OpenDoor} + @{#NAVIGATOR_FLAGS.UsePathgrid});

scripts/data/integration_tests/test_lua_api/player.lua

@@ -71,7 +71,6 @@ testing.registerLocalTest('findPath',
         local dst = util.vector3(4500, 4500, 700.216)
         local options = {
             agentBounds = types.Actor.getPathfindingAgentBounds(self),
-            stepSize = 50,
             includeFlags = nearby.NAVIGATOR_FLAGS.Walk + nearby.NAVIGATOR_FLAGS.Swim,
             areaCosts = {
                 water = 1,

scripts/data/morrowind_tests/player.lua

@@ -47,86 +47,30 @@ testing.registerLocalTest('Guard in Imperial Prison Ship should find path (#7241
         if agentBounds.shapeType == nearby.COLLISION_SHAPE_TYPE.Aabb then
             testing.expectThat(path, testing.elementsAreArray({
                 testing.closeToVector(util.vector3(34.29737091064453125, 806.3817138671875, 112.76610565185546875), 1e-1),
-                testing.closeToVector(util.vector3(30.4828090667724609375, 864.81732177734375, 112.76610565185546875), 1e-1),
-                testing.closeToVector(util.vector3(26.6682491302490234375, 923.25299072265625, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(22.85369110107421875, 981.6885986328125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(19.03913116455078125, 1040.124267578125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(15.22457122802734375, 1098.559814453125, 112.2945709228515625), 1e-1),
                 testing.closeToVector(util.vector3(15, 1102, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(15, 1102, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(15, 1102, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-67.99993896484375, 1108.4000244140625, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-112, 1110, 112.2945709228515625), 1e-1),
                 testing.closeToVector(util.vector3(-112, 1110, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-112, 1110, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-115.59993743896484375, 1360.0001220703125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-101.39704132080078125, 1416.811767578125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-43.336151123046875, 1424.44091796875, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-3.460088253021240234375, 1381.5552978515625, 92.34075927734375), 1e-1),
-                testing.closeToVector(util.vector3(3.82649898529052734375, 1323.4503173828125, 43.302886962890625), 1e-1),
-                testing.closeToVector(util.vector3(11.11308765411376953125, 1265.345458984375, -7.3479709625244140625), 1e-1),
-                testing.closeToVector(util.vector3(18.399677276611328125, 1207.240478515625, -54.67620849609375), 1e-1),
-                testing.closeToVector(util.vector3(25.6862640380859375, 1149.135498046875, -91.845550537109375), 1e-1),
-                testing.closeToVector(util.vector3(32.9728546142578125, 1091.030517578125, -97.08281707763671875), 1e-1),
-                testing.closeToVector(util.vector3(40.2594451904296875, 1032.9256591796875, -98.50542449951171875), 1e-1),
-                testing.closeToVector(util.vector3(47.546039581298828125, 974.82080078125, -98.50542449951171875), 1e-1),
-                testing.closeToVector(util.vector3(54.832630157470703125, 916.71588134765625, -98.50542449951171875), 1e-1),
-                testing.closeToVector(util.vector3(62.119220733642578125, 858.61102294921875, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(69.40581512451171875, 800.50616455078125, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(76.692413330078125, 742.4012451171875, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(79, 724, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(79, 724, -104.83390045166015625), 1e-1),
+                testing.closeToVector(util.vector3(-118, 1393, 112.2945709228515625), 1e-1),
+                testing.closeToVector(util.vector3(-67.99993896484375, 1421.2000732421875, 112.2945709228515625), 1e-1),
+                testing.closeToVector(util.vector3(-33.999935150146484375, 1414.4000244140625, 112.2945709228515625), 1e-1),
+                testing.closeToVector(util.vector3(-6.79993534088134765625, 1380.4000244140625, 85.094573974609375), 1e-1),
                 testing.closeToVector(util.vector3(79, 724, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(40.80001068115234375, 353.600006103515625, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(73.7038726806640625, 305.158203125, -104.83390045166015625), 1e-1),
                 testing.closeToVector(util.vector3(84, 290.000030517578125, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(84, 290.000030517578125, -104.58989715576171875), 1e-1),
-                testing.closeToVector(util.vector3(136.0000152587890625, 81.60001373291015625, -104.58989715576171875), 1e-1),
-                testing.closeToVector(util.vector3(89.15203094482421875, 46.464019775390625, -104.58989715576171875), 1e-1),
                 testing.closeToVector(util.vector3(83.552001953125, 42.26399993896484375, -104.58989715576171875), 1e-1),
-                testing.closeToVector(util.vector3(83.552001953125, 42.26399993896484375, -98.72841644287109375), 1e-1),
-                testing.closeToVector(util.vector3(115.60001373291015625, -27.1999359130859375, -98.72841644287109375), 1e-1),
-                testing.closeToVector(util.vector3(93.4945526123046875, -81.42742156982421875, -100.4057159423828125), 1e-1),
+                testing.closeToVector(util.vector3(89, -105, -98.72841644287109375), 1e-1),
                 testing.closeToVector(util.vector3(90, -90, -99.7056884765625), 1e-1),
             }))
         elseif agentBounds.shapeType == nearby.COLLISION_SHAPE_TYPE.Cylinder then
             testing.expectThat(path, testing.elementsAreArray({
                 testing.closeToVector(util.vector3(34.29737091064453125, 806.3817138671875, 112.76610565185546875), 1e-1),
-                testing.closeToVector(util.vector3(23.4630756378173828125, 863.9307861328125, 112.76610565185546875), 1e-1),
-                testing.closeToVector(util.vector3(12.628780364990234375, 921.4798583984375, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(1.79448258876800537109375, 979.0289306640625, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-9.0398197174072265625, 1036.5780029296875, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-19.8741359710693359375, 1094.126953125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-70.930450439453125, 1122.8067626953125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-121.98685455322265625, 1151.486328125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-121.020294189453125, 1210.038330078125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-120.0537261962890625, 1268.59033203125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-119.08716583251953125, 1327.142333984375, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-118.12059783935546875, 1385.6944580078125, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-118, 1393, 112.2945709228515625), 1e-1),
+                testing.closeToVector(util.vector3(-13.5999355316162109375, 1060.800048828125, 112.2945709228515625), 1e-1),
+                testing.closeToVector(util.vector3(-27.1999359130859375, 1081.2000732421875, 112.2945709228515625), 1e-1),
+                testing.closeToVector(util.vector3(-81.59993743896484375, 1128.800048828125, 112.2945709228515625), 1e-1),
+                testing.closeToVector(util.vector3(-101.99993896484375, 1156.0001220703125, 112.2945709228515625), 1e-1),
                 testing.closeToVector(util.vector3(-118, 1393, 112.2945709228515625), 1e-1),
-                testing.closeToVector(util.vector3(-118, 1393, 114.73973846435546875), 1e-1),
-                testing.closeToVector(util.vector3(27.200008392333984375, 1380.4000244140625, 114.73973846435546875), 1e-1),
-                testing.closeToVector(util.vector3(29.74369049072265625, 1321.8953857421875, 114.73973846435546875), 1e-1),
-                testing.closeToVector(util.vector3(32.287372589111328125, 1263.3907470703125, 114.73973846435546875), 1e-1),
-                testing.closeToVector(util.vector3(34.831058502197265625, 1204.885986328125, -57.1894378662109375), 1e-1),
-                testing.closeToVector(util.vector3(40.18719482421875, 1146.571533203125, -90.156890869140625), 1e-1),
-                testing.closeToVector(util.vector3(45.543331146240234375, 1088.2569580078125, -97.2764434814453125), 1e-1),
-                testing.closeToVector(util.vector3(50.89946746826171875, 1029.9423828125, -98.50542449951171875), 1e-1),
-                testing.closeToVector(util.vector3(56.255603790283203125, 971.62786865234375, -98.50542449951171875), 1e-1),
-                testing.closeToVector(util.vector3(61.6117401123046875, 913.31329345703125, -98.50542449951171875), 1e-1),
-                testing.closeToVector(util.vector3(66.9678802490234375, 854.998779296875, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(72.3240203857421875, 796.6842041015625, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(77.68015289306640625, 738.36968994140625, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(79, 724, -104.83390045166015625), 1e-1),
+                testing.closeToVector(util.vector3(7, 1470, 114.73973846435546875), 1e-1),
                 testing.closeToVector(util.vector3(79, 724, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(108.80001068115234375, 299.20001220703125, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(84, 290.000030517578125, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(84, 290.000030517578125, -104.83390045166015625), 1e-1),
                 testing.closeToVector(util.vector3(84, 290.000030517578125, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(108.80001068115234375, 54.4000091552734375, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(101.23966217041015625, -3.6698896884918212890625, -104.83390045166015625), 1e-1),
-                testing.closeToVector(util.vector3(93.6793060302734375, -61.7397918701171875, -104.83390045166015625), 1e-1),
+                testing.closeToVector(util.vector3(95, 27, -104.83390045166015625), 1e-1),
                 testing.closeToVector(util.vector3(90, -90, -104.83390045166015625), 1e-1),
             }))
         end