From 8fd6b37e34d42a05602ecdfb74925b10f5f4a5f3 Mon Sep 17 00:00:00 2001 From: elsid Date: Fri, 8 Mar 2019 20:01:50 +0300 Subject: [PATCH 1/3] Make path point tolerance depending on actor half extents --- apps/openmw/mwmechanics/aipackage.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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); From b9f21ec81a9c7e1d2403a14e8e937e10158de794 Mon Sep 17 00:00:00 2001 From: elsid Date: Fri, 8 Mar 2019 19:53:37 +0300 Subject: [PATCH 2/3] Use custom steps size to make smooth path depending on half extents --- apps/openmw/mwmechanics/pathfinding.cpp | 7 ++-- .../detournavigator/navigator.cpp | 34 ++++++++++--------- components/detournavigator/findsmoothpath.hpp | 11 +++--- components/detournavigator/navigator.hpp | 6 ++-- components/detournavigator/settingsutils.hpp | 5 +++ 5 files changed, 36 insertions(+), 27 deletions(-) diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index e31da94b15..c4d3a8e96c 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -301,8 +301,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 42b69d7f91..3211ddf00a 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; @@ -67,21 +69,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()); } @@ -90,7 +92,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) @@ -110,7 +112,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), @@ -160,7 +162,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), @@ -193,7 +195,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), @@ -244,7 +246,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), @@ -279,7 +281,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), @@ -336,7 +338,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), @@ -392,7 +394,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), @@ -445,7 +447,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), @@ -491,7 +493,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), @@ -537,7 +539,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), @@ -583,7 +585,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), @@ -632,7 +634,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()); From ea80a81538fdd116177a0501e8909926a3084589 Mon Sep 17 00:00:00 2001 From: elsid Date: Fri, 8 Mar 2019 19:58:42 +0300 Subject: [PATCH 3/3] Drop path points while tolerance allows --- apps/openmw/mwmechanics/pathfinding.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index c4d3a8e96c..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(); }