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());