From 9817f4ca9a90b8db9455bb870d614eb338cb05ff Mon Sep 17 00:00:00 2001 From: elsid Date: Sun, 18 Jun 2023 20:04:22 +0200 Subject: [PATCH] Find closest position on navmesh to start and end before poly path Start and end might not be located on navmesh and findPath may give wrong results. --- components/detournavigator/debug.cpp | 1 + components/detournavigator/findsmoothpath.hpp | 42 +++++++++++-------- components/detournavigator/status.hpp | 3 ++ files/lua_api/openmw/nearby.lua | 3 ++ 4 files changed, 32 insertions(+), 17 deletions(-) diff --git a/components/detournavigator/debug.cpp b/components/detournavigator/debug.cpp index 9aa28c84f2..898185ef75 100644 --- a/components/detournavigator/debug.cpp +++ b/components/detournavigator/debug.cpp @@ -36,6 +36,7 @@ namespace DetourNavigator OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(NavMeshNotFound) OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(StartPolygonNotFound) OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(EndPolygonNotFound) + OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(TargetPolygonNotFound) OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(MoveAlongSurfaceFailed) OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(FindPathOverPolygonsFailed) OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(InitNavMeshQueryFailed) diff --git a/components/detournavigator/findsmoothpath.hpp b/components/detournavigator/findsmoothpath.hpp index 23dc842426..8b5e863705 100644 --- a/components/detournavigator/findsmoothpath.hpp +++ b/components/detournavigator/findsmoothpath.hpp @@ -132,6 +132,7 @@ namespace DetourNavigator return static_cast(pathLen); } + // Iterate over the path to find smooth path on the detail mesh surface. template Status makeSmoothPath(const dtNavMeshQuery& navMeshQuery, const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end, const float stepSize, std::span polygonPath, std::size_t polygonPathSize, @@ -139,14 +140,8 @@ namespace DetourNavigator { assert(polygonPathSize <= polygonPath.size()); - // Iterate over the path to find smooth path on the detail mesh surface. - osg::Vec3f iterPos; - navMeshQuery.closestPointOnPoly(polygonPath.front(), start.ptr(), iterPos.ptr(), nullptr); - - osg::Vec3f targetPos; - navMeshQuery.closestPointOnPoly(polygonPath[polygonPathSize - 1], end.ptr(), targetPos.ptr(), nullptr); - constexpr float slop = 0.01f; + osg::Vec3f iterPos = start; *out++ = iterPos; @@ -158,7 +153,7 @@ namespace DetourNavigator { // Find location to steer towards. const auto steerTarget - = getSteerTarget(navMeshQuery, iterPos, targetPos, slop, polygonPath.data(), polygonPathSize); + = getSteerTarget(navMeshQuery, iterPos, end, slop, polygonPath.data(), polygonPathSize); if (!steerTarget) break; @@ -188,7 +183,7 @@ namespace DetourNavigator if (endOfPath && inRange(result->mResultPos, steerTarget->mSteerPos, slop)) { // Reached end of path. - iterPos = targetPos; + iterPos = end; *out++ = iterPos; ++smoothPathSize; break; @@ -264,17 +259,24 @@ namespace DetourNavigator constexpr float polyDistanceFactor = 4; const osg::Vec3f polyHalfExtents = halfExtents * polyDistanceFactor; - const dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, polyHalfExtents); - if (startRef == 0) + osg::Vec3f startNavMeshPos; + dtPolyRef startRef = 0; + if (const dtStatus status = navMeshQuery.findNearestPoly( + start.ptr(), polyHalfExtents.ptr(), &queryFilter, &startRef, startNavMeshPos.ptr()); + dtStatusFailed(status) || startRef == 0) return Status::StartPolygonNotFound; - const dtPolyRef endRef = findNearestPoly( - navMeshQuery, queryFilter, end, polyHalfExtents + osg::Vec3f(endTolerance, endTolerance, endTolerance)); - if (endRef == 0) + osg::Vec3f endNavMeshPos; + const osg::Vec3f endPolyHalfExtents = polyHalfExtents + osg::Vec3f(endTolerance, endTolerance, endTolerance); + dtPolyRef endRef; + if (const dtStatus status = navMeshQuery.findNearestPoly( + end.ptr(), endPolyHalfExtents.ptr(), &queryFilter, &endRef, endNavMeshPos.ptr()); + dtStatusFailed(status) || endRef == 0) return Status::EndPolygonNotFound; std::vector polygonPath(settings.mMaxPolygonPathSize); - const auto polygonPathSize = findPath(navMeshQuery, startRef, endRef, start, end, queryFilter, polygonPath); + const auto polygonPathSize + = findPath(navMeshQuery, startRef, endRef, startNavMeshPos, endNavMeshPos, queryFilter, polygonPath); if (!polygonPathSize.has_value()) return Status::FindPathOverPolygonsFailed; @@ -282,9 +284,15 @@ namespace DetourNavigator if (*polygonPathSize == 0) return Status::Success; + osg::Vec3f targetNavMeshPos; + if (const dtStatus status = navMeshQuery.closestPointOnPoly( + polygonPath[*polygonPathSize - 1], end.ptr(), targetNavMeshPos.ptr(), nullptr); + dtStatusFailed(status)) + return Status::TargetPolygonNotFound; + const bool partialPath = polygonPath[*polygonPathSize - 1] != endRef; - const Status smoothStatus = makeSmoothPath(navMeshQuery, queryFilter, start, end, stepSize, polygonPath, - *polygonPathSize, settings.mMaxSmoothPathSize, out); + const Status smoothStatus = makeSmoothPath(navMeshQuery, queryFilter, startNavMeshPos, targetNavMeshPos, + stepSize, polygonPath, *polygonPathSize, settings.mMaxSmoothPathSize, out); if (smoothStatus != Status::Success) return smoothStatus; diff --git a/components/detournavigator/status.hpp b/components/detournavigator/status.hpp index aeda99e82a..4735644c09 100644 --- a/components/detournavigator/status.hpp +++ b/components/detournavigator/status.hpp @@ -10,6 +10,7 @@ namespace DetourNavigator NavMeshNotFound, StartPolygonNotFound, EndPolygonNotFound, + TargetPolygonNotFound, MoveAlongSurfaceFailed, FindPathOverPolygonsFailed, InitNavMeshQueryFailed, @@ -29,6 +30,8 @@ namespace DetourNavigator return "polygon for start position is not found on navmesh"; case Status::EndPolygonNotFound: return "polygon for end position is not found on navmesh"; + case Status::TargetPolygonNotFound: + return "polygon for target position is not found on navmesh"; case Status::MoveAlongSurfaceFailed: return "move along surface on navmesh is failed"; case Status::FindPathOverPolygonsFailed: diff --git a/files/lua_api/openmw/nearby.lua b/files/lua_api/openmw/nearby.lua index 7638add551..7311188e3b 100644 --- a/files/lua_api/openmw/nearby.lua +++ b/files/lua_api/openmw/nearby.lua @@ -135,6 +135,9 @@ -- @field [parent=#FIND_PATH_STATUS] #number EndPolygonNotFound `destination` position is too far from available -- navigation mesh. The status may appear when navigation mesh is not fully generated or position is outside of covered -- area; +-- @field [parent=#FIND_PATH_STATUS] #number TargetPolygonNotFound adjusted `destination` position is too far from available +-- navigation mesh. The status may appear when navigation mesh is not fully generated or position is outside of covered +-- area; -- @field [parent=#FIND_PATH_STATUS] #number MoveAlongSurfaceFailed Found path couldn't be smoothed due to imperfect -- algorithm implementation or bad navigation mesh data; -- @field [parent=#FIND_PATH_STATUS] #number FindPathOverPolygonsFailed Path over navigation mesh from `source` to