diff --git a/apps/components_tests/detournavigator/navigator.cpp b/apps/components_tests/detournavigator/navigator.cpp index 0a78c33c9e..a6e1d26c79 100644 --- a/apps/components_tests/detournavigator/navigator.cpp +++ b/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()); } @@ -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( - findPath(*mNavigator, mAgentBounds, 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, @@ -487,8 +487,8 @@ namespace mStart.x() = 256; mEnd.x() = 256; - EXPECT_EQ( - findPath(*mNavigator, mAgentBounds, 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, @@ -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 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(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 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(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 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(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 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 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(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 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 { }; diff --git a/apps/openmw/mwlua/nearbybindings.cpp b/apps/openmw/mwlua/nearbybindings.cpp index a6d762499a..6c244a0fd4 100644 --- a/apps/openmw/mwlua/nearbybindings.cpp +++ b/apps/openmw/mwlua/nearbybindings.cpp @@ -233,6 +233,7 @@ namespace MWLua DetourNavigator::Flags includeFlags = defaultIncludeFlags; DetourNavigator::AreaCosts areaCosts{}; float destinationTolerance = 1; + std::vector checkpoints; if (options.has_value()) { @@ -258,13 +259,24 @@ namespace MWLua } if (const auto& v = options->get>("destinationTolerance")) destinationTolerance = *v; + if (const auto& t = options->get>("checkpoints")) + { + for (const auto& [k, v] : *t) + { + const int index = k.as(); + const osg::Vec3f position = v.as(); + if (index != static_cast(checkpoints.size() + 1)) + throw std::runtime_error("checkpoints is not an array"); + checkpoints.push_back(position); + } + } } std::vector path; - const DetourNavigator::Status status - = DetourNavigator::findPath(*MWBase::Environment::get().getWorld()->getNavigator(), agentBounds, - source, destination, includeFlags, areaCosts, destinationTolerance, std::back_inserter(path)); + const DetourNavigator::Status status = DetourNavigator::findPath( + *MWBase::Environment::get().getWorld()->getNavigator(), agentBounds, source, destination, + includeFlags, areaCosts, destinationTolerance, checkpoints, std::back_inserter(path)); sol::table result(lua, sol::create); LuaUtil::copyVectorToTable(path, result); diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp index b71c01397b..3fcb28307c 100644 --- a/apps/openmw/mwmechanics/aipackage.cpp +++ b/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; diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp index 40b8ee1a5a..71cc8b5e7d 100644 --- a/apps/openmw/mwmechanics/aiwander.cpp +++ b/apps/openmw/mwmechanics/aiwander.cpp @@ -242,20 +242,12 @@ namespace MWMechanics { const ESM::Pathgrid* pathgrid = MWBase::Environment::get().getESMStore()->get().search(*actor.getCell()->getCell()); - if (mUsePathgrid) - { - mPathFinder.buildPathByPathgrid( - pos.asVec3(), mDestination, actor.getCell(), getPathGridGraph(pathgrid)); - } - else - { - 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); - } + 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(); - auto& prng = world->getPrng(); + MWBase::World& world = *MWBase::Environment::get().getWorld(); + Misc::Rng::Generator& prng = world.getPrng(); const std::size_t randomAllowedPositionIndex = static_cast(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 ESM::Pathgrid* pathgrid = world->getStore().get().search(*actor.getCell()->getCell()); - mPathFinder.buildPathByPathgrid(start, randomAllowedPosition, actor.getCell(), getPathGridGraph(pathgrid)); + const MWWorld::Cell& cell = *actor.getCell()->getCell(); + const ESM::Pathgrid* pathgrid = world.getStore().get().search(cell); + const PathgridGraph& pathgridGraph = getPathGridGraph(pathgrid); - if (mPathFinder.isPathConstructed()) - { - 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; + const Misc::CoordinateConverter converter = Misc::makeCoordinateConverter(cell); + std::deque path + = pathgridGraph.aStarSearch(Misc::getClosestPoint(*pathgrid, converter.toLocalVec3(start)), + Misc::getClosestPoint(*pathgrid, converter.toLocalVec3(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 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) diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index 8a3d9116e0..165250c5c8 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/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 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 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> out) + PathType pathType, std::span checkpoints, + std::back_insert_iterator> out) { - const auto world = MWBase::Environment::get().getWorld(); - const auto navigator = world->getNavigator(); - const auto status = DetourNavigator::findPath( - *navigator, agentBounds, startPoint, endPoint, flags, areaCosts, endTolerance, out); + const MWBase::World& world = *MWBase::Environment::get().getWorld(); + const DetourNavigator::Navigator& navigator = *world.getNavigator(); + const DetourNavigator::Status status = DetourNavigator::findPath( + navigator, agentBounds, startPoint, endPoint, flags, areaCosts, endTolerance, checkpoints, out); if (pathType == PathType::Partial && status == DetourNavigator::Status::PartialPath) return DetourNavigator::Status::Success; diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index 77b2f83c12..55064d9e88 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -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 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 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> out); + PathType pathType, std::span checkpoints, + std::back_insert_iterator> out); }; } diff --git a/components/detournavigator/findsmoothpath.hpp b/components/detournavigator/findsmoothpath.hpp index e5efa8815f..d01b6bc1c7 100644 --- a/components/detournavigator/findsmoothpath.hpp +++ b/components/detournavigator/findsmoothpath.hpp @@ -13,7 +13,6 @@ #include -#include #include #include #include @@ -64,6 +63,25 @@ namespace DetourNavigator std::reference_wrapper mSettings; }; + template + class ToNavMeshCoordinatesSpan + { + public: + explicit ToNavMeshCoordinatesSpan(std::span 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 mSpan; + const RecastSettings& mSettings; + }; + inline std::optional findPolygonPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef, const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter, std::span pathBuffer) @@ -79,7 +97,7 @@ namespace DetourNavigator } Status makeSmoothPath(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& start, const osg::Vec3f& end, - std::span polygonPath, std::size_t polygonPathSize, std::size_t maxSmoothPathSize, + std::span polygonPath, std::size_t polygonPathSize, std::size_t maxSmoothPathSize, bool skipFirst, std::output_iterator 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(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 auto out) + float endTolerance, const ToNavMeshCoordinatesSpan& checkpoints, + std::output_iterator auto out) { dtQueryFilter queryFilter; queryFilter.setIncludeFlags(includeFlags); @@ -131,29 +150,66 @@ namespace DetourNavigator return Status::EndPolygonNotFound; std::vector polygonPath(settings.mMaxPolygonPathSize); - const auto polygonPathSize - = findPolygonPath(navMeshQuery, startRef, endRef, startNavMeshPos, endNavMeshPos, queryFilter, polygonPath); + std::span polygonPathBuffer = 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 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 toEndPathSize = findPolygonPath( + navMeshQuery, currentRef, endRef, currentNavMeshPos, endNavMeshPos, queryFilter, polygonPathBuffer); + + if (!toEndPathSize.has_value()) return Status::FindPathOverPolygonsFailed; - if (*polygonPathSize == 0) - return Status::Success; + if (*toEndPathSize == 0) + 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, startNavMeshPos, targetNavMeshPos, polygonPath, - *polygonPathSize, settings.mMaxSmoothPathSize, out); + const Status smoothStatus = makeSmoothPath(navMeshQuery, currentNavMeshPos, targetNavMeshPos, polygonPath, + *toEndPathSize, settings.mMaxSmoothPathSize, skipFirst, out); if (smoothStatus != Status::Success) return smoothStatus; - return partialPath ? Status::PartialPath : Status::Success; + return polygonPath[*toEndPathSize - 1] == endRef ? Status::Success : Status::PartialPath; } } diff --git a/components/detournavigator/navigatorutils.hpp b/components/detournavigator/navigatorutils.hpp index ca02682ecd..d3b8b5e35a 100644 --- a/components/detournavigator/navigatorutils.hpp +++ b/components/detournavigator/navigatorutils.hpp @@ -11,6 +11,7 @@ #include #include +#include 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 auto out) + std::span checkpoints, std::output_iterator 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); } /** diff --git a/files/lua_api/openmw/nearby.lua b/files/lua_api/openmw/nearby.lua index 29f1e79c24..a09e0cf50a 100644 --- a/files/lua_api/openmw/nearby.lua +++ b/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} diff --git a/scripts/data/integration_tests/test_lua_api/global.lua b/scripts/data/integration_tests/test_lua_api/global.lua index e5bfb5ef36..3a85e224d1 100644 --- a/scripts/data/integration_tests/test_lua_api/global.lua +++ b/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') diff --git a/scripts/data/integration_tests/test_lua_api/menu.lua b/scripts/data/integration_tests/test_lua_api/menu.lua index 3fc45bf2e7..8a2a278046 100644 --- a/scripts/data/integration_tests/test_lua_api/menu.lua +++ b/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') diff --git a/scripts/data/integration_tests/test_lua_api/player.lua b/scripts/data/integration_tests/test_lua_api/player.lua index 0b481fba2b..88c4d2ee0f 100644 --- a/scripts/data/integration_tests/test_lua_api/player.lua +++ b/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',