Build path over navmesh for wandering actors

Using a path over pathgrid as checkpoints.

This allows to avoid having paths going through obstacles if they are
placed over pathgrid points.

apps/components_tests/detournavigator/navigator.cpp

@@ -139,7 +139,7 @@ namespace
 
     TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty)
     {
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, 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>());
     }
@@ -147,7 +147,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, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::StartPolygonNotFound);
     }
 
@@ -156,7 +156,7 @@ namespace
         ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
         ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
         mNavigator->removeAgent(mAgentBounds);
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::StartPolygonNotFound);
     }
 
@@ -172,7 +172,7 @@ namespace
         updateGuard.reset();
         mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -194,7 +194,7 @@ namespace
         updateGuard.reset();
         mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mStart, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mStart, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath, ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125))) << mPath;
@@ -218,7 +218,7 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -237,7 +237,7 @@ namespace
 
         mPath.clear();
         mOut = std::back_inserter(mPath);
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -265,7 +265,7 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -285,7 +285,7 @@ namespace
 
         mPath.clear();
         mOut = std::back_inserter(mPath);
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -318,7 +318,7 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -386,7 +386,7 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -421,7 +421,7 @@ namespace
         mEnd.x() = 256;
         mEnd.z() = 300;
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_swim, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_swim, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -453,8 +453,8 @@ namespace
         mStart.x() = 256;
         mEnd.x() = 256;
 
-        EXPECT_EQ(
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance,
-            findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                      {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -487,8 +487,8 @@ namespace
         mStart.x() = 256;
         mEnd.x() = 256;
 
-        EXPECT_EQ(
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance,
-            findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut),
+                      {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -520,7 +520,7 @@ namespace
         mStart.x() = 256;
         mEnd.x() = 256;
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -549,7 +549,7 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -577,7 +577,7 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -658,7 +658,7 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -781,7 +781,7 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -806,7 +806,7 @@ namespace
         mNavigator->update(mPlayerPosition, nullptr);
         mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
             Status::PartialPath);
 
         EXPECT_THAT(mPath,
@@ -834,7 +834,7 @@ namespace
 
         const float endTolerance = 1000.0f;
 
-        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, endTolerance, mOut),
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mEnd, Flag_walk, mAreaCosts, endTolerance, {}, mOut),
             Status::Success);
 
         EXPECT_THAT(mPath,
@@ -979,6 +979,146 @@ namespace
         EXPECT_EQ(usedNavMeshTiles, 854);
     }
 
+    TEST_F(DetourNavigatorNavigatorTest, find_path_should_return_path_around_steep_mountains)
+    {
+        const std::array<float, 5 * 5> heightfieldData{ {
+            0, 0, 0, 0, 0, // row 0
+            0, 0, 0, 0, 0, // row 1
+            0, 0, 1000, 0, 0, // row 2
+            0, 0, 0, 0, 0, // row 3
+            0, 0, 0, 0, 0, // row 4
+        } };
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
+        const int cellSize = heightfieldTileSize * static_cast<int>(surface.mSize - 1);
+
+        ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
+        mNavigator->addHeightfield(mCellPosition, cellSize, surface, nullptr);
+        mNavigator->update(mPlayerPosition, nullptr);
+        mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
+
+        const osg::Vec3f start(56, 56, 12);
+        const osg::Vec3f end(464, 464, 12);
+
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, start, end, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
+            Status::Success);
+
+        EXPECT_THAT(mPath,
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 56.66664886474609375, 11.33333301544189453125),
+                Vec3fEq(396.666656494140625, 79.33331298828125, 11.33333301544189453125),
+                Vec3fEq(430.666656494140625, 113.33331298828125, 11.33333301544189453125),
+                Vec3fEq(463.999969482421875, 463.999969482421875, 11.33333301544189453125)))
+            << mPath;
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, find_path_should_return_path_around_steep_cliffs)
+    {
+        const std::array<float, 5 * 5> heightfieldData{ {
+            0, 0, 0, 0, 0, // row 0
+            0, 0, 0, 0, 0, // row 1
+            0, 0, -1000, 0, 0, // row 2
+            0, 0, 0, 0, 0, // row 3
+            0, 0, 0, 0, 0, // row 4
+        } };
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
+        const int cellSize = heightfieldTileSize * static_cast<int>(surface.mSize - 1);
+
+        ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
+        mNavigator->addHeightfield(mCellPosition, cellSize, surface, nullptr);
+        mNavigator->update(mPlayerPosition, nullptr);
+        mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
+
+        const osg::Vec3f start(56, 56, 12);
+        const osg::Vec3f end(464, 464, 12);
+
+        EXPECT_EQ(findPath(*mNavigator, mAgentBounds, start, end, Flag_walk, mAreaCosts, mEndTolerance, {}, mOut),
+            Status::Success);
+
+        EXPECT_THAT(mPath,
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 56.66664886474609375, 8.66659259796142578125),
+                Vec3fEq(385.33331298828125, 79.33331298828125, 8.66659259796142578125),
+                Vec3fEq(430.666656494140625, 124.66664886474609375, 8.66659259796142578125),
+                Vec3fEq(463.999969482421875, 463.999969482421875, 8.66659259796142578125)))
+            << mPath;
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, find_path_should_return_path_with_checkpoints)
+    {
+        const std::array<float, 5 * 5> heightfieldData{ {
+            0, 0, 0, 0, 0, // row 0
+            0, 0, 0, 0, 0, // row 1
+            0, 0, 1000, 0, 0, // row 2
+            0, 0, 0, 0, 0, // row 3
+            0, 0, 0, 0, 0, // row 4
+        } };
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
+        const int cellSize = heightfieldTileSize * static_cast<int>(surface.mSize - 1);
+
+        ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
+        mNavigator->addHeightfield(mCellPosition, cellSize, surface, nullptr);
+        mNavigator->update(mPlayerPosition, nullptr);
+        mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
+
+        const std::vector<osg::Vec3f> checkpoints = {
+            osg::Vec3f(400, 70, 12),
+        };
+
+        const osg::Vec3f start(56, 56, 12);
+        const osg::Vec3f end(464, 464, 12);
+
+        EXPECT_EQ(
+            findPath(*mNavigator, mAgentBounds, start, end, Flag_walk, mAreaCosts, mEndTolerance, checkpoints, mOut),
+            Status::Success);
+
+        EXPECT_THAT(mPath,
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 56.66664886474609375, 11.33333301544189453125),
+                Vec3fEq(400, 70, 11.33333301544189453125),
+                Vec3fEq(430.666656494140625, 113.33331298828125, 11.33333301544189453125),
+                Vec3fEq(463.999969482421875, 463.999969482421875, 11.33333301544189453125)))
+            << mPath;
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, find_path_should_skip_unreachable_checkpoints)
+    {
+        const std::array<float, 5 * 5> heightfieldData{ {
+            0, 0, 0, 0, 0, // row 0
+            0, 0, 0, 0, 0, // row 1
+            0, 0, 1000, 0, 0, // row 2
+            0, 0, 0, 0, 0, // row 3
+            0, 0, 0, 0, 0, // row 4
+        } };
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
+        const int cellSize = heightfieldTileSize * static_cast<int>(surface.mSize - 1);
+
+        ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
+        mNavigator->addHeightfield(mCellPosition, cellSize, surface, nullptr);
+        mNavigator->update(mPlayerPosition, nullptr);
+        mNavigator->wait(WaitConditionType::allJobsDone, &mListener);
+
+        const std::vector<osg::Vec3f> checkpoints = {
+            osg::Vec3f(400, 70, 10000),
+            osg::Vec3f(256, 256, 1000),
+            osg::Vec3f(-1000, -1000, 0),
+        };
+
+        const osg::Vec3f start(56, 56, 12);
+        const osg::Vec3f end(464, 464, 12);
+
+        EXPECT_EQ(
+            findPath(*mNavigator, mAgentBounds, start, end, Flag_walk, mAreaCosts, mEndTolerance, checkpoints, mOut),
+            Status::Success);
+
+        EXPECT_THAT(mPath,
+            ElementsAre( //
+                Vec3fEq(56.66664886474609375, 56.66664886474609375, 11.33333301544189453125),
+                Vec3fEq(396.666656494140625, 79.33331298828125, 11.33333301544189453125),
+                Vec3fEq(430.666656494140625, 113.33331298828125, 11.33333301544189453125),
+                Vec3fEq(463.999969482421875, 463.999969482421875, 11.33333301544189453125)))
+            << mPath;
+    }
+
     struct DetourNavigatorNavigatorNotSupportedAgentBoundsTest : TestWithParam<AgentBounds>
     {
     };

apps/openmw/mwlua/nearbybindings.cpp

@@ -233,6 +233,7 @@ namespace MWLua
                   DetourNavigator::Flags includeFlags = defaultIncludeFlags;
                   DetourNavigator::AreaCosts areaCosts{};
                   float destinationTolerance = 1;
+                  std::vector<osg::Vec3f> checkpoints;
 
                   if (options.has_value())
                   {
@@ -258,13 +259,24 @@ namespace MWLua
                       }
                       if (const auto& v = options->get<sol::optional<float>>("destinationTolerance"))
                           destinationTolerance = *v;
+                      if (const auto& t = options->get<sol::optional<sol::table>>("checkpoints"))
+                      {
+                          for (const auto& [k, v] : *t)
+                          {
+                              const int index = k.as<int>();
+                              const osg::Vec3f position = v.as<osg::Vec3f>();
+                              if (index != static_cast<int>(checkpoints.size() + 1))
+                                  throw std::runtime_error("checkpoints is not an array");
+                              checkpoints.push_back(position);
+                          }
+                      }
                   }
 
                   std::vector<osg::Vec3f> path;
 
-                  const DetourNavigator::Status status
+                  const DetourNavigator::Status status = DetourNavigator::findPath(
-                      = DetourNavigator::findPath(*MWBase::Environment::get().getWorld()->getNavigator(), agentBounds,
+                      *MWBase::Environment::get().getWorld()->getNavigator(), agentBounds, source, destination,
-                          source, destination, includeFlags, areaCosts, destinationTolerance, std::back_inserter(path));
+                      includeFlags, areaCosts, destinationTolerance, checkpoints, std::back_inserter(path));
 
                   sol::table result(lua, sol::create);
                   LuaUtil::copyVectorToTable(path, result);

apps/openmw/mwmechanics/aipackage.cpp

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

apps/openmw/mwmechanics/aiwander.cpp

@@ -242,20 +242,12 @@ namespace MWMechanics
         {
             const ESM::Pathgrid* pathgrid
                 = MWBase::Environment::get().getESMStore()->get<ESM::Pathgrid>().search(*actor.getCell()->getCell());
-            if (mUsePathgrid)
+            const auto agentBounds = MWBase::Environment::get().getWorld()->getPathfindingAgentBounds(actor);
-            {
+            constexpr float endTolerance = 0;
-                mPathFinder.buildPathByPathgrid(
+            const DetourNavigator::Flags navigatorFlags = getNavigatorFlags(actor);
-                    pos.asVec3(), mDestination, actor.getCell(), getPathGridGraph(pathgrid));
+            const DetourNavigator::AreaCosts areaCosts = getAreaCosts(actor, navigatorFlags);
-            }
+            mPathFinder.buildPath(actor, pos.asVec3(), mDestination, getPathGridGraph(pathgrid), agentBounds,
-            else
+                navigatorFlags, areaCosts, endTolerance, PathType::Full);
-            {
-                const auto agentBounds = MWBase::Environment::get().getWorld()->getPathfindingAgentBounds(actor);
-                constexpr float endTolerance = 0;
-                const DetourNavigator::Flags navigatorFlags = getNavigatorFlags(actor);
-                const DetourNavigator::AreaCosts areaCosts = getAreaCosts(actor, navigatorFlags);
-                mPathFinder.buildPath(actor, pos.asVec3(), mDestination, getPathGridGraph(pathgrid), agentBounds,
-                    navigatorFlags, areaCosts, endTolerance, PathType::Full);
-            }
 
             if (mPathFinder.isPathConstructed())
                 storage.setState(AiWanderStorage::Wander_Walking, !mUsePathgrid);
@@ -633,38 +625,64 @@ namespace MWMechanics
     void AiWander::setPathToAnAllowedPosition(
         const MWWorld::Ptr& actor, AiWanderStorage& storage, const ESM::Position& actorPos)
     {
-        auto world = MWBase::Environment::get().getWorld();
+        MWBase::World& world = *MWBase::Environment::get().getWorld();
-        auto& prng = world->getPrng();
+        Misc::Rng::Generator& prng = world.getPrng();
         const std::size_t randomAllowedPositionIndex
             = static_cast<std::size_t>(Misc::Rng::rollDice(storage.mAllowedPositions.size(), prng));
         const osg::Vec3f randomAllowedPosition = storage.mAllowedPositions[randomAllowedPositionIndex];
 
         const osg::Vec3f start = actorPos.asVec3();
 
-        // don't take shortcuts for wandering
+        const MWWorld::Cell& cell = *actor.getCell()->getCell();
-        const ESM::Pathgrid* pathgrid = world->getStore().get<ESM::Pathgrid>().search(*actor.getCell()->getCell());
+        const ESM::Pathgrid* pathgrid = world.getStore().get<ESM::Pathgrid>().search(cell);
-        mPathFinder.buildPathByPathgrid(start, randomAllowedPosition, actor.getCell(), getPathGridGraph(pathgrid));
+        const PathgridGraph& pathgridGraph = getPathGridGraph(pathgrid);
 
-        if (mPathFinder.isPathConstructed())
+        const Misc::CoordinateConverter converter = Misc::makeCoordinateConverter(cell);
-        {
+        std::deque<ESM::Pathgrid::Point> path
-            mDestination = randomAllowedPosition;
+            = pathgridGraph.aStarSearch(Misc::getClosestPoint(*pathgrid, converter.toLocalVec3(start)),
-            mHasDestination = true;
+                Misc::getClosestPoint(*pathgrid, converter.toLocalVec3(randomAllowedPosition)));
-            mUsePathgrid = true;
-            // Remove this position as an option and add back the previously used position (stops NPC from picking the
-            // same position):
-            storage.mAllowedPositions.erase(storage.mAllowedPositions.begin() + randomAllowedPositionIndex);
-            // check if mCurrentPosition was taken out of mAllowedPositions
-            if (storage.mTrimCurrentPosition && storage.mAllowedPositions.size() > 1)
-                storage.mTrimCurrentPosition = false;
-            else
-                storage.mAllowedPositions.push_back(storage.mCurrentPosition);
-            storage.mCurrentPosition = randomAllowedPosition;
 
-            storage.setState(AiWanderStorage::Wander_Walking);
-        }
         // Choose a different position and delete this one from possible positions because it is uncreachable:
-        else
+        if (path.empty())
+        {
             storage.mAllowedPositions.erase(storage.mAllowedPositions.begin() + randomAllowedPositionIndex);
+            return;
+        }
+
+        // Drop nearest pathgrid point.
+        path.pop_front();
+
+        std::vector<osg::Vec3f> checkpoints(path.size());
+        for (std::size_t i = 0; i < path.size(); ++i)
+            checkpoints[i] = Misc::Convert::makeOsgVec3f(converter.toWorldPoint(path[i]));
+
+        const DetourNavigator::AgentBounds agentBounds = world.getPathfindingAgentBounds(actor);
+        const DetourNavigator::Flags flags = getNavigatorFlags(actor);
+        const DetourNavigator::AreaCosts areaCosts = getAreaCosts(actor, flags);
+        constexpr float endTolerance = 0;
+        mPathFinder.buildPath(actor, start, randomAllowedPosition, pathgridGraph, agentBounds, flags, areaCosts,
+            endTolerance, PathType::Full, checkpoints);
+
+        if (!mPathFinder.isPathConstructed())
+        {
+            storage.mAllowedPositions.erase(storage.mAllowedPositions.begin() + randomAllowedPositionIndex);
+            return;
+        }
+
+        mDestination = randomAllowedPosition;
+        mHasDestination = true;
+        mUsePathgrid = true;
+        // Remove this position as an option and add back the previously used position (stops NPC from picking the
+        // same position):
+        storage.mAllowedPositions.erase(storage.mAllowedPositions.begin() + randomAllowedPositionIndex);
+        // check if mCurrentPosition was taken out of mAllowedPositions
+        if (storage.mTrimCurrentPosition && storage.mAllowedPositions.size() > 1)
+            storage.mTrimCurrentPosition = false;
+        else
+            storage.mAllowedPositions.push_back(storage.mCurrentPosition);
+        storage.mCurrentPosition = randomAllowedPosition;
+
+        storage.setState(AiWanderStorage::Wander_Walking);
     }
 
     void AiWander::stopWalking(const MWWorld::Ptr& actor)

apps/openmw/mwmechanics/pathfinding.cpp

@@ -360,26 +360,16 @@ namespace MWMechanics
         mConstructed = true;
     }
 
-    void PathFinder::buildPathByPathgrid(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-        const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph)
-    {
-        mPath.clear();
-        mCell = cell;
-
-        buildPathByPathgridImpl(startPoint, endPoint, pathgridGraph, std::back_inserter(mPath));
-
-        mConstructed = !mPath.empty();
-    }
-
     void PathFinder::buildPathByNavMesh(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint,
         const osg::Vec3f& endPoint, const DetourNavigator::AgentBounds& agentBounds, const DetourNavigator::Flags flags,
-        const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType)
+        const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType,
+        std::span<const osg::Vec3f> checkpoints)
     {
         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, agentBounds, flags,
-            areaCosts, endTolerance, pathType, std::back_inserter(mPath));
+            areaCosts, endTolerance, pathType, checkpoints, std::back_inserter(mPath));
 
         if (status != DetourNavigator::Status::Success)
             mPath.clear();
@@ -393,7 +383,7 @@ namespace MWMechanics
     void PathFinder::buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
         const PathgridGraph& pathgridGraph, const DetourNavigator::AgentBounds& agentBounds,
         const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
-        PathType pathType)
+        PathType pathType, std::span<const osg::Vec3f> checkpoints)
     {
         mPath.clear();
         mCell = actor.getCell();
@@ -403,7 +393,7 @@ namespace MWMechanics
         if (!actor.getClass().isPureWaterCreature(actor) && !actor.getClass().isPureFlyingCreature(actor))
         {
             status = buildPathByNavigatorImpl(actor, startPoint, endPoint, agentBounds, flags, areaCosts, endTolerance,
-                pathType, std::back_inserter(mPath));
+                pathType, checkpoints, std::back_inserter(mPath));
             if (status != DetourNavigator::Status::Success)
                 mPath.clear();
         }
@@ -412,7 +402,7 @@ namespace MWMechanics
             && (flags & DetourNavigator::Flag_usePathgrid) == 0)
         {
             status = buildPathByNavigatorImpl(actor, startPoint, endPoint, agentBounds,
-                flags | DetourNavigator::Flag_usePathgrid, areaCosts, endTolerance, pathType,
+                flags | DetourNavigator::Flag_usePathgrid, areaCosts, endTolerance, pathType, checkpoints,
                 std::back_inserter(mPath));
             if (status != DetourNavigator::Status::Success)
                 mPath.clear();
@@ -430,12 +420,13 @@ namespace MWMechanics
     DetourNavigator::Status PathFinder::buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor,
         const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const DetourNavigator::AgentBounds& agentBounds,
         const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
-        PathType pathType, std::back_insert_iterator<std::deque<osg::Vec3f>> out)
+        PathType pathType, std::span<const osg::Vec3f> checkpoints,
+        std::back_insert_iterator<std::deque<osg::Vec3f>> out)
     {
-        const auto world = MWBase::Environment::get().getWorld();
+        const MWBase::World& world = *MWBase::Environment::get().getWorld();
-        const auto navigator = world->getNavigator();
+        const DetourNavigator::Navigator& navigator = *world.getNavigator();
-        const auto status = DetourNavigator::findPath(
+        const DetourNavigator::Status status = DetourNavigator::findPath(
-            *navigator, agentBounds, startPoint, endPoint, flags, areaCosts, endTolerance, out);
+            navigator, agentBounds, startPoint, endPoint, flags, areaCosts, endTolerance, checkpoints, out);
 
         if (pathType == PathType::Partial && status == DetourNavigator::Status::PartialPath)
             return DetourNavigator::Status::Success;

apps/openmw/mwmechanics/pathfinding.hpp

@@ -4,6 +4,7 @@
 #include <cassert>
 #include <deque>
 #include <iterator>
+#include <span>
 
 #include <osg/Vec3f>
 
@@ -102,18 +103,15 @@ namespace MWMechanics
 
         void buildStraightPath(const osg::Vec3f& endPoint);
 
-        void buildPathByPathgrid(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
-            const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph);
-
         void buildPathByNavMesh(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint,
             const osg::Vec3f& endPoint, const DetourNavigator::AgentBounds& agentBounds,
             const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
-            PathType pathType);
+            PathType pathType, std::span<const osg::Vec3f> checkpoints = {});
 
         void buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
             const PathgridGraph& pathgridGraph, const DetourNavigator::AgentBounds& agentBounds,
             const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
-            PathType pathType);
+            PathType pathType, std::span<const osg::Vec3f> checkpoints = {});
 
         void buildLimitedPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
             const PathgridGraph& pathgridGraph, const DetourNavigator::AgentBounds& agentBounds,
@@ -156,7 +154,8 @@ namespace MWMechanics
         [[nodiscard]] DetourNavigator::Status buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor,
             const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const DetourNavigator::AgentBounds& agentBounds,
             const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
-            PathType pathType, std::back_insert_iterator<std::deque<osg::Vec3f>> out);
+            PathType pathType, std::span<const osg::Vec3f> checkpoints,
+            std::back_insert_iterator<std::deque<osg::Vec3f>> out);
     };
 }
 

components/detournavigator/findsmoothpath.hpp

@@ -13,7 +13,6 @@
 
 #include <osg/Vec3f>
 
-#include <array>
 #include <cassert>
 #include <functional>
 #include <iterator>
@@ -64,6 +63,25 @@ namespace DetourNavigator
         std::reference_wrapper<const RecastSettings> mSettings;
     };
 
+    template <class T>
+    class ToNavMeshCoordinatesSpan
+    {
+    public:
+        explicit ToNavMeshCoordinatesSpan(std::span<T> span, const RecastSettings& settings)
+            : mSpan(span)
+            , mSettings(settings)
+        {
+        }
+
+        std::size_t size() const noexcept { return mSpan.size(); }
+
+        T operator[](std::size_t i) const noexcept { return toNavMeshCoordinates(mSettings, mSpan[i]); }
+
+    private:
+        std::span<T> mSpan;
+        const RecastSettings& mSettings;
+    };
+
     inline std::optional<std::size_t> findPolygonPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef,
         const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter,
         std::span<dtPolyRef> pathBuffer)
@@ -79,7 +97,7 @@ namespace DetourNavigator
     }
 
     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,
+        std::span<dtPolyRef> polygonPath, std::size_t polygonPathSize, std::size_t maxSmoothPathSize, bool skipFirst,
         std::output_iterator<osg::Vec3f> auto& out)
     {
         assert(polygonPathSize <= polygonPath.size());
@@ -95,7 +113,7 @@ namespace DetourNavigator
             dtStatusFailed(status))
             return Status::FindStraightPathFailed;
 
-        for (int i = 0; i < cornersCount; ++i)
+        for (int i = skipFirst ? 1 : 0; i < cornersCount; ++i)
             *out++ = Misc::Convert::makeOsgVec3f(&cornerVertsBuffer[static_cast<std::size_t>(i) * 3]);
 
         return Status::Success;
@@ -103,7 +121,8 @@ namespace DetourNavigator
 
     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, std::output_iterator<osg::Vec3f> auto out)
+        float endTolerance, const ToNavMeshCoordinatesSpan<const osg::Vec3f>& checkpoints,
+        std::output_iterator<osg::Vec3f> auto out)
     {
         dtQueryFilter queryFilter;
         queryFilter.setIncludeFlags(includeFlags);
@@ -131,29 +150,66 @@ namespace DetourNavigator
             return Status::EndPolygonNotFound;
 
         std::vector<dtPolyRef> polygonPath(settings.mMaxPolygonPathSize);
-        const auto polygonPathSize
+        std::span<dtPolyRef> polygonPathBuffer = polygonPath;
-            = findPolygonPath(navMeshQuery, startRef, endRef, startNavMeshPos, endNavMeshPos, queryFilter, polygonPath);
+        dtPolyRef currentRef = startRef;
+        osg::Vec3f currentNavMeshPos = startNavMeshPos;
+        bool skipFirst = false;
 
-        if (!polygonPathSize.has_value())
+        for (std::size_t i = 0; i < checkpoints.size(); ++i)
+        {
+            const osg::Vec3f checkpointPos = checkpoints[i];
+            osg::Vec3f checkpointNavMeshPos;
+            dtPolyRef checkpointRef;
+            if (const dtStatus status = navMeshQuery.findNearestPoly(checkpointPos.ptr(), polyHalfExtents.ptr(),
+                    &queryFilter, &checkpointRef, checkpointNavMeshPos.ptr());
+                dtStatusFailed(status) || checkpointRef == 0)
+                continue;
+
+            const std::optional<std::size_t> toCheckpointPathSize = findPolygonPath(navMeshQuery, currentRef,
+                checkpointRef, currentNavMeshPos, checkpointNavMeshPos, queryFilter, polygonPath);
+
+            if (!toCheckpointPathSize.has_value())
+                continue;
+
+            if (*toCheckpointPathSize == 0)
+                continue;
+
+            if (polygonPath[*toCheckpointPathSize - 1] != checkpointRef)
+                continue;
+
+            const Status smoothStatus = makeSmoothPath(navMeshQuery, currentNavMeshPos, checkpointNavMeshPos,
+                polygonPath, *toCheckpointPathSize, settings.mMaxSmoothPathSize, skipFirst, out);
+
+            if (smoothStatus != Status::Success)
+                return smoothStatus;
+
+            currentRef = checkpointRef;
+            currentNavMeshPos = checkpointNavMeshPos;
+            skipFirst = true;
+        }
+
+        const std::optional<std::size_t> toEndPathSize = findPolygonPath(
+            navMeshQuery, currentRef, endRef, currentNavMeshPos, endNavMeshPos, queryFilter, polygonPathBuffer);
+
+        if (!toEndPathSize.has_value())
             return Status::FindPathOverPolygonsFailed;
 
-        if (*polygonPathSize == 0)
+        if (*toEndPathSize == 0)
-            return Status::Success;
+            return currentRef == endRef ? Status::Success : Status::PartialPath;
 
         osg::Vec3f targetNavMeshPos;
         if (const dtStatus status = navMeshQuery.closestPointOnPoly(
-                polygonPath[*polygonPathSize - 1], end.ptr(), targetNavMeshPos.ptr(), nullptr);
+                polygonPath[*toEndPathSize - 1], end.ptr(), targetNavMeshPos.ptr(), nullptr);
             dtStatusFailed(status))
             return Status::TargetPolygonNotFound;
 
-        const bool partialPath = polygonPath[*polygonPathSize - 1] != endRef;
+        const Status smoothStatus = makeSmoothPath(navMeshQuery, currentNavMeshPos, targetNavMeshPos, polygonPath,
-        const Status smoothStatus = makeSmoothPath(navMeshQuery, startNavMeshPos, targetNavMeshPos, polygonPath,
+            *toEndPathSize, settings.mMaxSmoothPathSize, skipFirst, out);
-            *polygonPathSize, settings.mMaxSmoothPathSize, out);
 
         if (smoothStatus != Status::Success)
             return smoothStatus;
 
-        return partialPath ? Status::PartialPath : Status::Success;
+        return polygonPath[*toEndPathSize - 1] == endRef ? Status::Success : Status::PartialPath;
     }
 }
 

components/detournavigator/navigatorutils.hpp

@@ -11,6 +11,7 @@
 
 #include <iterator>
 #include <optional>
+#include <span>
 
 namespace DetourNavigator
 {
@@ -21,13 +22,13 @@ namespace DetourNavigator
      * @param end path at given point.
      * @param includeFlags setup allowed navmesh areas.
      * @param out the beginning of the destination range.
-     * @param endTolerance defines maximum allowed distance to end path point in addition to agentHalfExtents
+     * @param endTolerance defines maximum allowed distance to end path point in addition to agentHalfExtents.
+     * @param checkpoints is a sequence of positions the path should go over if possible.
      * @return Status.
-     * Equal to out if no path is found.
      */
     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,
-        std::output_iterator<osg::Vec3f> auto out)
+        std::span<const osg::Vec3f> checkpoints, std::output_iterator<osg::Vec3f> auto out)
     {
         const auto navMesh = navigator.getNavMesh(agentBounds);
         if (navMesh == nullptr)
@@ -37,7 +38,8 @@ namespace DetourNavigator
         const auto locked = navMesh->lock();
         return findSmoothPath(locked->getQuery(), toNavMeshCoordinates(settings.mRecast, agentBounds.mHalfExtents),
             toNavMeshCoordinates(settings.mRecast, start), toNavMeshCoordinates(settings.mRecast, end), includeFlags,
-            areaCosts, settings.mDetour, endTolerance, outTransform);
+            areaCosts, settings.mDetour, endTolerance, ToNavMeshCoordinatesSpan(checkpoints, settings.mRecast),
+            outTransform);
     }
 
     /**

files/lua_api/openmw/nearby.lua

@@ -180,6 +180,7 @@
 -- @field [parent=#FindPathOptions] #AreaCosts areaCosts a table defining relative cost for each type of area.
 -- @field [parent=#FindPathOptions] #number destinationTolerance a floating point number representing maximum allowed
 -- distance between destination and a nearest point on the navigation mesh in addition to agent size (default: 1).
+-- @field [parent=#FindPathOptions] #table checkpoints an array of positions to build path over if possible.
 
 ---
 -- A table of parameters for @{#nearby.findRandomPointAroundCircle} and @{#nearby.castNavigationRay}

scripts/data/integration_tests/test_lua_api/global.lua

@@ -315,6 +315,7 @@ registerPlayerTest('player rotation')
 registerPlayerTest('player forward running')
 registerPlayerTest('player diagonal walking')
 registerPlayerTest('findPath')
+registerPlayerTest('findPath with checkpoints')
 registerPlayerTest('findRandomPointAroundCircle')
 registerPlayerTest('castNavigationRay')
 registerPlayerTest('findNearestNavMeshPosition')

scripts/data/integration_tests/test_lua_api/menu.lua

@@ -82,6 +82,7 @@ registerGlobalTest('player rotation', 'rotating player should not lead to nan ro
 registerGlobalTest('player forward running')
 registerGlobalTest('player diagonal walking')
 registerGlobalTest('findPath')
+registerGlobalTest('findPath with checkpoints')
 registerGlobalTest('findRandomPointAroundCircle')
 registerGlobalTest('castNavigationRay')
 registerGlobalTest('findNearestNavMeshPosition')

scripts/data/integration_tests/test_lua_api/player.lua

@@ -179,6 +179,39 @@ testing.registerLocalTest('findPath',
         testing.expectEqual(status, nearby.FIND_PATH_STATUS.Success, 'Status')
         testing.expectLessOrEqual((path[#path] - dst):length(), 1,
             'Last path point '  .. testing.formatActualExpected(path[#path], dst))
+        testing.expectThat(path, matchers.equalTo({
+            matchers.closeToVector(util.vector3(4096, 4096, 1746.27099609375), 1e-1),
+            matchers.closeToVector(util.vector3(4500, 4500, 1745.95263671875), 1e-1),
+        }))
+    end)
+
+testing.registerLocalTest('findPath with checkpoints',
+    function()
+        local src = util.vector3(4096, 4096, 1745)
+        local dst = util.vector3(4500, 4500, 1745.95263671875)
+        local options = {
+            agentBounds = types.Actor.getPathfindingAgentBounds(self),
+            includeFlags = nearby.NAVIGATOR_FLAGS.Walk + nearby.NAVIGATOR_FLAGS.Swim,
+            areaCosts = {
+                water = 1,
+                door = 2,
+                ground = 1,
+                pathgrid = 1,
+            },
+            destinationTolerance = 1,
+            checkpoints = {
+                util.vector3(4200, 4100, 1750),
+            },
+        }
+        local status, path = nearby.findPath(src, dst, options)
+        testing.expectEqual(status, nearby.FIND_PATH_STATUS.Success, 'Status')
+        testing.expectLessOrEqual((path[#path] - dst):length(), 1,
+            'Last path point '  .. testing.formatActualExpected(path[#path], dst))
+        testing.expectThat(path, matchers.equalTo({
+            matchers.closeToVector(util.vector3(4096, 4096, 1746.27099609375), 1e-1),
+            matchers.closeToVector(util.vector3(4200, 4100, 1749.5076904296875), 1e-1),
+            matchers.closeToVector(util.vector3(4500, 4500, 1745.95263671875), 1e-1),
+        }))
     end)
 
 testing.registerLocalTest('findRandomPointAroundCircle',