diff --git a/apps/openmw/mwmechanics/aicombat.cpp b/apps/openmw/mwmechanics/aicombat.cpp index f6123c12c0..4b490d7bc8 100644 --- a/apps/openmw/mwmechanics/aicombat.cpp +++ b/apps/openmw/mwmechanics/aicombat.cpp @@ -269,7 +269,8 @@ namespace MWMechanics const auto navigatorFlags = getNavigatorFlags(actor); const auto areaCosts = getAreaCosts(actor); const auto pathGridGraph = getPathGridGraph(actor.getCell()); - mPathFinder.buildPath(actor, vActorPos, vTargetPos, actor.getCell(), pathGridGraph, halfExtents, navigatorFlags, areaCosts); + mPathFinder.buildPath(actor, vActorPos, vTargetPos, actor.getCell(), pathGridGraph, halfExtents, + navigatorFlags, areaCosts, storage.mAttackRange, PathType::Full); if (!mPathFinder.isPathConstructed()) { @@ -281,7 +282,8 @@ namespace MWMechanics if (hit.has_value() && (*hit - vTargetPos).length() <= rangeAttack) { // If the point is close enough, try to find a path to that point. - mPathFinder.buildPath(actor, vActorPos, *hit, actor.getCell(), pathGridGraph, halfExtents, navigatorFlags, areaCosts); + mPathFinder.buildPath(actor, vActorPos, *hit, actor.getCell(), pathGridGraph, halfExtents, + navigatorFlags, areaCosts, storage.mAttackRange, PathType::Full); if (mPathFinder.isPathConstructed()) { // If path to that point is found use it as custom destination. diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp index 8ad9447514..46e4729a8a 100644 --- a/apps/openmw/mwmechanics/aipackage.cpp +++ b/apps/openmw/mwmechanics/aipackage.cpp @@ -88,7 +88,8 @@ void MWMechanics::AiPackage::reset() mObstacleCheck.clear(); } -bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance) +bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, + float destTolerance, float endTolerance, PathType pathType) { const Misc::TimerStatus timerStatus = mReaction.update(duration); @@ -133,7 +134,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& { const auto pathfindingHalfExtents = world->getPathfindingHalfExtents(actor); mPathFinder.buildLimitedPath(actor, position, dest, actor.getCell(), getPathGridGraph(actor.getCell()), - pathfindingHalfExtents, getNavigatorFlags(actor), getAreaCosts(actor)); + pathfindingHalfExtents, getNavigatorFlags(actor), getAreaCosts(actor), endTolerance, pathType); mRotateOnTheRunChecks = 3; // give priority to go directly on target if there is minimal opportunity diff --git a/apps/openmw/mwmechanics/aipackage.hpp b/apps/openmw/mwmechanics/aipackage.hpp index 6d8af0d92b..5270b74166 100644 --- a/apps/openmw/mwmechanics/aipackage.hpp +++ b/apps/openmw/mwmechanics/aipackage.hpp @@ -129,7 +129,8 @@ namespace MWMechanics protected: /// Handles path building and shortcutting with obstacles avoiding /** \return If the actor has arrived at his destination **/ - bool pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance = 0.0f); + bool pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, + float destTolerance = 0.0f, float endTolerance = 0.0f, PathType pathType = PathType::Full); /// Check if there aren't any obstacles along the path to make shortcut possible /// If a shortcut is possible then path will be cleared and filled with the destination point. diff --git a/apps/openmw/mwmechanics/aipursue.cpp b/apps/openmw/mwmechanics/aipursue.cpp index 05605529ad..2d896ddbdc 100644 --- a/apps/openmw/mwmechanics/aipursue.cpp +++ b/apps/openmw/mwmechanics/aipursue.cpp @@ -53,7 +53,7 @@ bool AiPursue::execute (const MWWorld::Ptr& actor, CharacterController& characte const float pathTolerance = 100.f; // check the true distance in case the target is far away in Z-direction - bool reached = pathTo(actor, dest, duration, pathTolerance) && + bool reached = pathTo(actor, dest, duration, pathTolerance, (actorPos - dest).length(), PathType::Partial) && std::abs(dest.z() - actorPos.z()) < pathTolerance; if (reached) diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp index 24fdff7065..19365bcb08 100644 --- a/apps/openmw/mwmechanics/aiwander.cpp +++ b/apps/openmw/mwmechanics/aiwander.cpp @@ -201,8 +201,10 @@ namespace MWMechanics else { const osg::Vec3f halfExtents = MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(actor); + constexpr float endTolerance = 0; mPathFinder.buildPath(actor, pos.asVec3(), mDestination, actor.getCell(), - getPathGridGraph(actor.getCell()), halfExtents, getNavigatorFlags(actor), getAreaCosts(actor)); + getPathGridGraph(actor.getCell()), halfExtents, getNavigatorFlags(actor), getAreaCosts(actor), + endTolerance, PathType::Full); } if (mPathFinder.isPathConstructed()) @@ -361,11 +363,13 @@ namespace MWMechanics if (isAreaOccupiedByOtherActor(actor, mDestination)) continue; + constexpr float endTolerance = 0; + if (isWaterCreature || isFlyingCreature) mPathFinder.buildStraightPath(mDestination); else mPathFinder.buildPathByNavMesh(actor, currentPosition, mDestination, halfExtents, navigatorFlags, - areaCosts); + areaCosts, endTolerance, PathType::Full); if (mPathFinder.isPathConstructed()) { diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index 2f8e830434..c61b64f411 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -364,13 +364,13 @@ namespace MWMechanics void PathFinder::buildPathByNavMesh(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, - const DetourNavigator::AreaCosts& areaCosts) + const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType) { mPath.clear(); // If it's not possible to build path over navmesh due to disabled navmesh generation fallback to straight path DetourNavigator::Status status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, - areaCosts, std::back_inserter(mPath)); + areaCosts, endTolerance, pathType, std::back_inserter(mPath)); if (status != DetourNavigator::Status::Success) mPath.clear(); @@ -383,7 +383,8 @@ namespace MWMechanics void PathFinder::buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, - const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts) + const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance, + PathType pathType) { mPath.clear(); mCell = cell; @@ -392,7 +393,8 @@ namespace MWMechanics if (!actor.getClass().isPureWaterCreature(actor) && !actor.getClass().isPureFlyingCreature(actor)) { - status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, areaCosts, std::back_inserter(mPath)); + status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, areaCosts, + endTolerance, pathType, std::back_inserter(mPath)); if (status != DetourNavigator::Status::Success) mPath.clear(); } @@ -400,7 +402,7 @@ namespace MWMechanics if (status != DetourNavigator::Status::NavMeshNotFound && mPath.empty()) { status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, - flags | DetourNavigator::Flag_usePathgrid, areaCosts, std::back_inserter(mPath)); + flags | DetourNavigator::Flag_usePathgrid, areaCosts, endTolerance, pathType, std::back_inserter(mPath)); if (status != DetourNavigator::Status::Success) mPath.clear(); } @@ -416,12 +418,17 @@ namespace MWMechanics DetourNavigator::Status PathFinder::buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, - const DetourNavigator::AreaCosts& areaCosts, std::back_insert_iterator> out) + const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType, + std::back_insert_iterator> out) { const auto world = MWBase::Environment::get().getWorld(); const auto stepSize = getPathStepSize(actor); const auto navigator = world->getNavigator(); - const auto status = navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, areaCosts, out); + const auto status = navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, areaCosts, + endTolerance, out); + + if (pathType == PathType::Partial && status == DetourNavigator::Status::PartialPath) + return DetourNavigator::Status::Success; if (status != DetourNavigator::Status::Success) { @@ -449,8 +456,9 @@ namespace MWMechanics const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); std::deque prePath; auto prePathInserter = std::back_inserter(prePath); + const float endTolerance = 0; const auto status = navigator->findPath(halfExtents, stepSize, startPoint, mPath.front(), flags, areaCosts, - prePathInserter); + endTolerance, prePathInserter); if (status == DetourNavigator::Status::NavMeshNotFound) return; @@ -475,7 +483,8 @@ namespace MWMechanics void PathFinder::buildLimitedPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, - const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts) + const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance, + PathType pathType) { const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); const auto maxDistance = std::min( @@ -485,8 +494,9 @@ namespace MWMechanics const auto startToEnd = endPoint - startPoint; const auto distance = startToEnd.length(); if (distance <= maxDistance) - return buildPath(actor, startPoint, endPoint, cell, pathgridGraph, halfExtents, flags, areaCosts); + return buildPath(actor, startPoint, endPoint, cell, pathgridGraph, halfExtents, flags, areaCosts, + endTolerance, pathType); const auto end = startPoint + startToEnd * maxDistance / distance; - buildPath(actor, startPoint, end, cell, pathgridGraph, halfExtents, flags, areaCosts); + buildPath(actor, startPoint, end, cell, pathgridGraph, halfExtents, flags, areaCosts, endTolerance, pathType); } } diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index 0ada08d3fb..f0a5040334 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -70,6 +70,12 @@ namespace MWMechanics // magnitude of pits/obstacles is defined by PATHFIND_Z_REACH bool checkWayIsClear(const osg::Vec3f& from, const osg::Vec3f& to, float offsetXY); + enum class PathType + { + Full, + Partial, + }; + class PathFinder { public: @@ -93,18 +99,20 @@ namespace MWMechanics void buildPathByNavMesh(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, - const DetourNavigator::AreaCosts& areaCosts); + const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType); void buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, - const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts); + const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance, + PathType pathType); void buildPathByNavMeshToNextPoint(const MWWorld::ConstPtr& actor, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts); void buildLimitedPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, - const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts); + const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance, + PathType pathType); /// Remove front point if exist and within tolerance void update(const osg::Vec3f& position, float pointTolerance, float destinationTolerance, @@ -212,7 +220,7 @@ namespace MWMechanics [[nodiscard]] DetourNavigator::Status buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, - const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, + const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType, std::back_insert_iterator> out); }; } diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp index 35fde5b657..62d3f9de04 100644 --- a/apps/openmw_test_suite/detournavigator/navigator.cpp +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -46,6 +46,7 @@ namespace const osg::Vec2i mCellPosition {0, 0}; const int mHeightfieldTileSize = ESM::Land::REAL_SIZE / (ESM::Land::LAND_SIZE - 1); const osg::Vec3f mShift {0, 0, 0}; + const float mEndTolerance = 0; DetourNavigatorNavigatorTest() : mPlayerPosition(0, 0, 0) @@ -135,7 +136,7 @@ namespace TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty) { - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), Status::NavMeshNotFound); EXPECT_EQ(mPath, std::deque()); } @@ -143,7 +144,7 @@ namespace TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception) { mNavigator->addAgent(mAgentHalfExtents); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), Status::StartPolygonNotFound); } @@ -152,7 +153,7 @@ namespace mNavigator->addAgent(mAgentHalfExtents); mNavigator->addAgent(mAgentHalfExtents); mNavigator->removeAgent(mAgentHalfExtents); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), Status::StartPolygonNotFound); } @@ -172,7 +173,8 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204.0000152587890625, 204, 1.99998295307159423828125), @@ -219,7 +221,8 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 1.99998295307159423828125), @@ -252,7 +255,8 @@ namespace mPath.clear(); mOut = std::back_inserter(mPath); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 1.99998295307159423828125), @@ -301,7 +305,8 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 1.99998295307159423828125), @@ -337,7 +342,8 @@ namespace mPath.clear(); mOut = std::back_inserter(mPath); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 1.99998295307159423828125), @@ -393,7 +399,8 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 1.999981403350830078125), @@ -479,7 +486,8 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 1.99997997283935546875), @@ -530,7 +538,8 @@ namespace mEnd.x() = 0; mEnd.z() = 300; - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(0, 204, 185.33331298828125), @@ -574,7 +583,7 @@ namespace mStart.x() = 0; mEnd.x() = 0; - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( @@ -619,7 +628,7 @@ namespace mStart.x() = 0; mEnd.x() = 0; - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( @@ -664,7 +673,8 @@ namespace mStart.x() = 0; mEnd.x() = 0; - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(0, 204, -98.000030517578125), @@ -712,7 +722,8 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 1.99998295307159423828125), @@ -764,7 +775,8 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 1.99998295307159423828125), @@ -858,7 +870,8 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 1.99998295307159423828125), @@ -1006,7 +1019,8 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-204, 204, 101.99999237060546875), @@ -1033,4 +1047,98 @@ namespace Vec3fEq(204, -204, 101.99999237060546875) )) << mPath; } + + TEST_F(DetourNavigatorNavigatorTest, for_not_reachable_destination_find_path_should_provide_partial_path) + { + const std::array heightfieldData {{ + 0, 0, 0, 0, 0, + 0, -25, -25, -25, -25, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + }}; + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + + CollisionShapeInstance compound(std::make_unique()); + compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(204, -204, 0)), + new btBoxShape(btVector3(200, 200, 1000))); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(mListener, WaitConditionType::allJobsDone); + + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), + Status::PartialPath); + + EXPECT_THAT(mPath, ElementsAre( + Vec3fEq(-204, 204, -2.666739940643310546875), + Vec3fEq(-193.730682373046875, 177.59320068359375, -3.9535934925079345703125), + Vec3fEq(-183.4613800048828125, 151.1864166259765625, -5.240451335906982421875), + Vec3fEq(-173.1920623779296875, 124.77962493896484375, -6.527309417724609375), + Vec3fEq(-162.922760009765625, 98.37282562255859375, -7.814167022705078125), + Vec3fEq(-152.6534423828125, 71.96602630615234375, -9.898590087890625), + Vec3fEq(-142.384124755859375, 45.559230804443359375, -17.641445159912109375), + Vec3fEq(-132.1148223876953125, 19.152431488037109375, -25.3843059539794921875), + Vec3fEq(-121.8455047607421875, -7.254369258880615234375, -27.97742462158203125), + Vec3fEq(-111.57619476318359375, -33.66117095947265625, -16.974590301513671875), + Vec3fEq(-101.30689239501953125, -60.06797027587890625, -5.9717559814453125), + Vec3fEq(-91.0375823974609375, -86.47476959228515625, -2.6667339801788330078125), + Vec3fEq(-80.76827239990234375, -112.88156890869140625, -2.6667339801788330078125), + Vec3fEq(-70.49897003173828125, -139.2883758544921875, -2.6667339801788330078125), + Vec3fEq(-60.229663848876953125, -165.6951751708984375, -2.6667339801788330078125), + Vec3fEq(-49.96035003662109375, -192.1019744873046875, -2.6667339801788330078125), + Vec3fEq(-45.333343505859375, -204, -2.6667339801788330078125) + )) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, end_tolerance_should_extent_available_destinations) + { + const std::array heightfieldData {{ + 0, 0, 0, 0, 0, + 0, -25, -25, -25, -25, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + 0, -25, -100, -100, -100, + }}; + const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + + CollisionShapeInstance compound(std::make_unique()); + compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(204, -204, 0)), + new btBoxShape(btVector3(100, 100, 1000))); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(mListener, WaitConditionType::allJobsDone); + + const float endTolerance = 1000.0f; + + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, endTolerance, mOut), + Status::Success); + + EXPECT_THAT(mPath, ElementsAre( + Vec3fEq(-204, 204, -2.666739940643310546875), + Vec3fEq(-188.745635986328125, 180.1236114501953125, -4.578275203704833984375), + Vec3fEq(-173.49127197265625, 156.247222900390625, -6.489814281463623046875), + Vec3fEq(-158.2369232177734375, 132.370819091796875, -8.4013538360595703125), + Vec3fEq(-142.9825592041015625, 108.49443817138671875, -10.31289386749267578125), + Vec3fEq(-127.7281951904296875, 84.6180419921875, -17.4810466766357421875), + Vec3fEq(-112.47383880615234375, 60.7416534423828125, -27.6026020050048828125), + Vec3fEq(-97.21947479248046875, 36.865261077880859375, -37.724163055419921875), + Vec3fEq(-81.965118408203125, 12.98886966705322265625, -47.84572601318359375), + Vec3fEq(-66.71075439453125, -10.887523651123046875, -46.691577911376953125), + Vec3fEq(-51.45639801025390625, -34.763916015625, -32.085445404052734375), + Vec3fEq(-36.202037811279296875, -58.640308380126953125, -28.5217914581298828125), + Vec3fEq(-20.947673797607421875, -82.5167083740234375, -32.16143035888671875), + Vec3fEq(-5.693310260772705078125, -106.393096923828125, -35.8010711669921875), + Vec3fEq(9.56105327606201171875, -130.2694854736328125, -29.6399688720703125), + Vec3fEq(24.8154163360595703125, -154.1458740234375, -17.6428318023681640625), + Vec3fEq(40.0697784423828125, -178.0222625732421875, -10.46006107330322265625), + Vec3fEq(55.3241424560546875, -201.8986663818359375, -3.297139644622802734375), + Vec3fEq(56.66666412353515625, -204, -2.6667373180389404296875) + )) << mPath; + } } diff --git a/components/detournavigator/debug.hpp b/components/detournavigator/debug.hpp index a17eec16a7..2128f96be4 100644 --- a/components/detournavigator/debug.hpp +++ b/components/detournavigator/debug.hpp @@ -34,6 +34,7 @@ namespace DetourNavigator switch (value) { OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(Success) + OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(PartialPath) OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(NavMeshNotFound) OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(StartPolygonNotFound) OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(EndPolygonNotFound) diff --git a/components/detournavigator/findrandompointaroundcircle.cpp b/components/detournavigator/findrandompointaroundcircle.cpp index ed73dca61a..a3407a61c3 100644 --- a/components/detournavigator/findrandompointaroundcircle.cpp +++ b/components/detournavigator/findrandompointaroundcircle.cpp @@ -19,7 +19,7 @@ namespace DetourNavigator dtQueryFilter queryFilter; queryFilter.setIncludeFlags(includeFlags); - dtPolyRef startRef = findNearestPolyExpanding(navMeshQuery, queryFilter, start, halfExtents); + dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, halfExtents * 4); if (startRef == 0) return std::optional(); diff --git a/components/detournavigator/findsmoothpath.cpp b/components/detournavigator/findsmoothpath.cpp index 6598263398..14fe696f10 100644 --- a/components/detournavigator/findsmoothpath.cpp +++ b/components/detournavigator/findsmoothpath.cpp @@ -146,16 +146,13 @@ namespace DetourNavigator return result; } - dtPolyRef findNearestPolyExpanding(const dtNavMeshQuery& query, const dtQueryFilter& filter, + dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter, const osg::Vec3f& center, const osg::Vec3f& halfExtents) { dtPolyRef ref = 0; - for (int i = 0; i < 3; ++i) - { - const dtStatus status = query.findNearestPoly(center.ptr(), (halfExtents * (1 << i)).ptr(), &filter, &ref, nullptr); - if (!dtStatusFailed(status) && ref != 0) - break; - } + const dtStatus status = query.findNearestPoly(center.ptr(), halfExtents.ptr(), &filter, &ref, nullptr); + if (!dtStatusSucceed(status)) + return 0; return ref; } } diff --git a/components/detournavigator/findsmoothpath.hpp b/components/detournavigator/findsmoothpath.hpp index db2219b2cd..8eb0bf9f34 100644 --- a/components/detournavigator/findsmoothpath.hpp +++ b/components/detournavigator/findsmoothpath.hpp @@ -99,7 +99,7 @@ namespace DetourNavigator return dtStatusSucceed(status); } - dtPolyRef findNearestPolyExpanding(const dtNavMeshQuery& query, const dtQueryFilter& filter, + dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter, const osg::Vec3f& center, const osg::Vec3f& halfExtents); struct MoveAlongSurfaceResult @@ -256,7 +256,7 @@ namespace DetourNavigator template Status findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, const float stepSize, const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts, - const Settings& settings, OutputIterator& out) + const Settings& settings, float endTolerance, OutputIterator& out) { dtNavMeshQuery navMeshQuery; if (!initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes)) @@ -269,11 +269,15 @@ namespace DetourNavigator queryFilter.setAreaCost(AreaType_pathgrid, areaCosts.mPathgrid); queryFilter.setAreaCost(AreaType_ground, areaCosts.mGround); - dtPolyRef startRef = findNearestPolyExpanding(navMeshQuery, queryFilter, start, halfExtents); + constexpr float polyDistanceFactor = 4; + const osg::Vec3f polyHalfExtents = halfExtents * polyDistanceFactor; + + const dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, polyHalfExtents); if (startRef == 0) return Status::StartPolygonNotFound; - dtPolyRef endRef = findNearestPolyExpanding(navMeshQuery, queryFilter, end, halfExtents); + const dtPolyRef endRef = findNearestPoly(navMeshQuery, queryFilter, end, + polyHalfExtents + osg::Vec3f(endTolerance, endTolerance, endTolerance)); if (endRef == 0) return Status::EndPolygonNotFound; @@ -283,12 +287,18 @@ namespace DetourNavigator if (!polygonPath) return Status::FindPathOverPolygonsFailed; - if (polygonPath->empty() || polygonPath->back() != endRef) + if (polygonPath->empty()) return Status::Success; + const bool partialPath = polygonPath->back() != endRef; auto outTransform = OutputTransformIterator(out, settings); - return makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize, std::move(*polygonPath), - settings.mMaxSmoothPathSize, outTransform); + const Status smoothStatus = makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize, + std::move(*polygonPath), settings.mMaxSmoothPathSize, outTransform); + + if (smoothStatus != Status::Success) + return smoothStatus; + + return partialPath ? Status::PartialPath : Status::Success; } } diff --git a/components/detournavigator/navigator.hpp b/components/detournavigator/navigator.hpp index 1a78b602cd..d2e77892b9 100644 --- a/components/detournavigator/navigator.hpp +++ b/components/detournavigator/navigator.hpp @@ -173,13 +173,14 @@ namespace DetourNavigator * @param end path at given point. * @param includeFlags setup allowed surfaces for actor to walk. * @param out the beginning of the destination range. + * @param endTolerance defines maximum allowed distance to end path point in addition to agentHalfExtents * @return Output iterator to the element in the destination range, one past the last element of found path. * Equal to out if no path is found. */ template Status findPath(const osg::Vec3f& agentHalfExtents, const float stepSize, const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const DetourNavigator::AreaCosts& areaCosts, - OutputIterator& out) const + float endTolerance, OutputIterator& out) const { static_assert( std::is_same< @@ -194,7 +195,7 @@ namespace DetourNavigator const auto settings = getSettings(); return findSmoothPath(navMesh->lockConst()->getImpl(), toNavMeshCoordinates(settings, agentHalfExtents), toNavMeshCoordinates(settings, stepSize), toNavMeshCoordinates(settings, start), - toNavMeshCoordinates(settings, end), includeFlags, areaCosts, settings, out); + toNavMeshCoordinates(settings, end), includeFlags, areaCosts, settings, endTolerance, out); } /** diff --git a/components/detournavigator/status.hpp b/components/detournavigator/status.hpp index 3715489acc..5f01f04758 100644 --- a/components/detournavigator/status.hpp +++ b/components/detournavigator/status.hpp @@ -6,6 +6,7 @@ namespace DetourNavigator enum class Status { Success, + PartialPath, NavMeshNotFound, StartPolygonNotFound, EndPolygonNotFound, @@ -21,6 +22,8 @@ namespace DetourNavigator { case Status::Success: return "success"; + case Status::PartialPath: + return "partial path is found"; case Status::NavMeshNotFound: return "navmesh is not found"; case Status::StartPolygonNotFound: