From fea4fb6e692ddeb76e357a282414c3c5523a4ae9 Mon Sep 17 00:00:00 2001 From: elsid Date: Wed, 18 Aug 2021 22:46:14 +0200 Subject: [PATCH] Make AiPursue path destination to be as close as possible to target Even when target is not reachable actor will try to run there either because target navmesh polygon is selected within extended area or because partial path is built to the closest possible polygon. --- apps/openmw/mwmechanics/aicombat.cpp | 6 +- apps/openmw/mwmechanics/aipackage.cpp | 5 +- apps/openmw/mwmechanics/aipackage.hpp | 3 +- apps/openmw/mwmechanics/aipursue.cpp | 2 +- apps/openmw/mwmechanics/aiwander.cpp | 8 +- apps/openmw/mwmechanics/pathfinding.cpp | 32 ++-- apps/openmw/mwmechanics/pathfinding.hpp | 16 +- .../detournavigator/navigator.cpp | 144 +++++++++++++++--- components/detournavigator/debug.hpp | 1 + .../findrandompointaroundcircle.cpp | 2 +- components/detournavigator/findsmoothpath.cpp | 11 +- components/detournavigator/findsmoothpath.hpp | 24 ++- components/detournavigator/navigator.hpp | 5 +- components/detournavigator/status.hpp | 3 + 14 files changed, 204 insertions(+), 58 deletions(-) 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: