diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp index 9b01353c77..90da9d1609 100644 --- a/apps/openmw/mwmechanics/aipackage.cpp +++ b/apps/openmw/mwmechanics/aipackage.cpp @@ -167,7 +167,9 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& mTimer = 0; } - const float pointTolerance = std::max(MIN_TOLERANCE, std::min(actor.getClass().getSpeed(actor), DEFAULT_TOLERANCE)); + const float actorTolerance = 2 * actor.getClass().getSpeed(actor) * duration + + 1.2 * std::max(halfExtents.x(), halfExtents.y()); + const float pointTolerance = std::max(MIN_TOLERANCE, actorTolerance); mPathFinder.update(position, pointTolerance, DEFAULT_TOLERANCE); diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index e31da94b15..e0285be928 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -262,9 +262,10 @@ namespace MWMechanics if (mPath.empty()) return; - const auto tolerance = mPath.size() > 1 ? pointTolerance : destinationTolerance; + while (mPath.size() > 1 && sqrDistanceIgnoreZ(mPath.front(), position) < pointTolerance * pointTolerance) + mPath.pop_front(); - if (sqrDistanceIgnoreZ(mPath.front(), position) < tolerance * tolerance) + if (mPath.size() == 1 && sqrDistanceIgnoreZ(mPath.front(), position) < destinationTolerance * destinationTolerance) mPath.pop_front(); } @@ -301,8 +302,11 @@ namespace MWMechanics { try { - const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); - navigator->findPath(halfExtents, startPoint, endPoint, flags, out); + const auto world = MWBase::Environment::get().getWorld(); + const auto realHalfExtents = world->getHalfExtents(actor); // This may differ from halfExtents argument + const auto stepSize = 2 * std::max(realHalfExtents.x(), realHalfExtents.y()); + const auto navigator = world->getNavigator(); + navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, out); } catch (const DetourNavigator::NavigatorException& exception) { diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp index 9daef6f64e..2bf7bf6438 100644 --- a/apps/openmw_test_suite/detournavigator/navigator.cpp +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -27,6 +27,7 @@ namespace osg::Vec3f mEnd; std::deque mPath; std::back_insert_iterator> mOut; + float mStepSize; DetourNavigatorNavigatorTest() : mPlayerPosition(0, 0, 0) @@ -34,6 +35,7 @@ namespace , mStart(-215, 215, 1) , mEnd(215, -215, 1) , mOut(mPath) + , mStepSize(28.333332061767578125f) { mSettings.mEnableWriteRecastMeshToFile = false; mSettings.mEnableWriteNavMeshToFile = false; @@ -68,21 +70,21 @@ namespace TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty) { - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mPath, std::deque()); } TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception) { mNavigator->addAgent(mAgentHalfExtents); - EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), NavigatorException); + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), NavigatorException); } TEST_F(DetourNavigatorNavigatorTest, find_path_for_removed_agent_should_return_empty) { mNavigator->addAgent(mAgentHalfExtents); mNavigator->removeAgent(mAgentHalfExtents); - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mPath, std::deque()); } @@ -91,7 +93,7 @@ namespace mNavigator->addAgent(mAgentHalfExtents); mNavigator->addAgent(mAgentHalfExtents); mNavigator->removeAgent(mAgentHalfExtents); - EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), NavigatorException); + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), NavigatorException); } TEST_F(DetourNavigatorNavigatorTest, update_then_find_path_should_return_path) @@ -111,7 +113,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(-215, 215, 1.85963428020477294921875), @@ -161,7 +163,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(-215, 215, 1.85963428020477294921875), @@ -194,7 +196,7 @@ namespace mNavigator->wait(); mPath.clear(); - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(-215, 215, 1.87826788425445556640625), @@ -245,7 +247,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(-215, 215, 1.87826788425445556640625), @@ -280,7 +282,7 @@ namespace mNavigator->wait(); mPath.clear(); - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(-215, 215, 1.85963428020477294921875), @@ -337,7 +339,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(-215, 215, 1.96328866481781005859375), @@ -393,7 +395,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(-215, 215, 1.9393787384033203125), @@ -446,7 +448,7 @@ namespace mEnd.x() = 0; mEnd.z() = 300; - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_swim, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mOut); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(0, 215, 185.33331298828125), @@ -492,7 +494,7 @@ namespace mStart.x() = 0; mEnd.x() = 0; - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_swim | Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mOut); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(0, 215, -94.75363922119140625), @@ -538,7 +540,7 @@ namespace mStart.x() = 0; mEnd.x() = 0; - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_swim | Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mOut); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(0, 215, -94.75363922119140625), @@ -584,7 +586,7 @@ namespace mStart.x() = 0; mEnd.x() = 0; - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(0, 215, -94.75363922119140625), @@ -633,7 +635,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(-215, 215, 1.85963428020477294921875), diff --git a/components/detournavigator/findsmoothpath.hpp b/components/detournavigator/findsmoothpath.hpp index ce5febebe0..e988dedb73 100644 --- a/components/detournavigator/findsmoothpath.hpp +++ b/components/detournavigator/findsmoothpath.hpp @@ -166,7 +166,7 @@ namespace DetourNavigator template OutputIterator makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, - const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end, + const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end, const float stepSize, std::vector polygonPath, std::size_t maxSmoothPathSize, OutputIterator out) { // Iterate over the path to find smooth path on the detail mesh surface. @@ -176,7 +176,6 @@ namespace DetourNavigator osg::Vec3f targetPos; navMeshQuery.closestPointOnPoly(polygonPath.back(), end.ptr(), targetPos.ptr(), 0); - const float STEP_SIZE = 0.5f; const float SLOP = 0.01f; *out++ = iterPos; @@ -200,10 +199,10 @@ namespace DetourNavigator const osg::Vec3f delta = steerTarget->steerPos - iterPos; float len = delta.length(); // If the steer target is end of path or off-mesh link, do not move past the location. - if ((endOfPath || offMeshConnection) && len < STEP_SIZE) + if ((endOfPath || offMeshConnection) && len < stepSize) len = 1; else - len = STEP_SIZE / len; + len = stepSize / len; const osg::Vec3f moveTgt = iterPos + delta * len; const auto result = moveAlongSurface(navMeshQuery, polygonPath.front(), iterPos, moveTgt, filter, 16); @@ -273,7 +272,7 @@ namespace DetourNavigator } template - OutputIterator findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, + OutputIterator findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, const float stepSize, const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const Settings& settings, OutputIterator out) { @@ -315,7 +314,7 @@ namespace DetourNavigator if (polygonPath.empty() || polygonPath.back() != endRef) return out; - makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, std::move(polygonPath), + makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize, std::move(polygonPath), settings.mMaxSmoothPathSize, OutputTransformIterator(out, settings)); return out; diff --git a/components/detournavigator/navigator.hpp b/components/detournavigator/navigator.hpp index a06d97c563..a146fe0fb9 100644 --- a/components/detournavigator/navigator.hpp +++ b/components/detournavigator/navigator.hpp @@ -160,7 +160,7 @@ namespace DetourNavigator * @throws InvalidArgument if there is no navmesh for given agentHalfExtents. */ template - OutputIterator findPath(const osg::Vec3f& agentHalfExtents, const osg::Vec3f& start, + OutputIterator findPath(const osg::Vec3f& agentHalfExtents, const float stepSize, const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, OutputIterator out) const { static_assert( @@ -175,8 +175,8 @@ namespace DetourNavigator return out; const auto settings = getSettings(); return findSmoothPath(navMesh.lock()->getValue(), toNavMeshCoordinates(settings, agentHalfExtents), - toNavMeshCoordinates(settings, start), toNavMeshCoordinates(settings, end), includeFlags, - settings, out); + toNavMeshCoordinates(settings, stepSize), toNavMeshCoordinates(settings, start), + toNavMeshCoordinates(settings, end), includeFlags, settings, out); } /** diff --git a/components/detournavigator/settingsutils.hpp b/components/detournavigator/settingsutils.hpp index d96ea53cc2..a22205b2aa 100644 --- a/components/detournavigator/settingsutils.hpp +++ b/components/detournavigator/settingsutils.hpp @@ -31,6 +31,11 @@ namespace DetourNavigator return agentHalfExtents.x() * settings.mRecastScaleFactor; } + inline float toNavMeshCoordinates(const Settings& settings, float value) + { + return value * settings.mRecastScaleFactor; + } + inline osg::Vec3f toNavMeshCoordinates(const Settings& settings, osg::Vec3f position) { std::swap(position.y(), position.z());