mirror/OpenMW
1
0
Fork 0
mirror of https://gitlab.com/OpenMW/openmw.git synced 2025-04-07 13:20:25 +00:00
Code Issues Projects Releases Wiki Activity

Merge branch 'fix_find_path_crash' into 'master'

Browse Source 
Fix crash in finding path over navmesh (#6338)

Closes #6338

See merge request OpenMW/openmw!1296
This commit is contained in:
psi29a 2021-10-17 12:15:14 +00:00
commit 62dfbb33d9
2 changed files with 55 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
components/detournavigator/findsmoothpath.cpp
View File

@ -7,12 +7,16 @@
namespace DetourNavigator namespace DetourNavigator
{ {
std::vector<dtPolyRef> fixupCorridor(const std::vector<dtPolyRef>& path, const std::vector<dtPolyRef>& visited) std::size_t fixupCorridor(dtPolyRef* path, std::size_t pathSize, const std::vector<dtPolyRef>& visited)
{ {
std::vector<dtPolyRef>::const_reverse_iterator furthestVisited; std::vector<dtPolyRef>::const_reverse_iterator furthestVisited;
// Find furthest common polygon. // Find furthest common polygon.
const auto it = std::find_if(path.rbegin(), path.rend(), [&] (dtPolyRef pathValue) const auto begin = path;
const auto end = path + pathSize;
const std::reverse_iterator rbegin(end);
const std::reverse_iterator rend(begin);
const auto it = std::find_if(rbegin, rend, [&] (dtPolyRef pathValue)
{ {
const auto it = std::find(visited.rbegin(), visited.rend(), pathValue); const auto it = std::find(visited.rbegin(), visited.rend(), pathValue);
if (it == visited.rend()) if (it == visited.rend())
@ -22,8 +26,8 @@ namespace DetourNavigator
}); });
// If no intersection found just return current path. // If no intersection found just return current path.
if (it == path.rend()) if (it == rend)
return path; return pathSize;
const auto furthestPath = it.base() - 1; const auto furthestPath = it.base() - 1;
// Concatenate paths. // Concatenate paths.
@ -34,36 +38,22 @@ namespace DetourNavigator
// ^ furthestPath // ^ furthestPath
// result: x b_n ... b_1 D // result: x b_n ... b_1 D
std::vector<dtPolyRef> result; auto newEnd = std::copy(visited.rbegin(), furthestVisited + 1, begin);
result.reserve(static_cast<std::size_t>(furthestVisited - visited.rbegin()) newEnd = std::copy(furthestPath + 1, end, newEnd);
+ static_cast<std::size_t>(path.end() - furthestPath) - 1);
std::copy(visited.rbegin(), furthestVisited + 1, std::back_inserter(result));
std::copy(furthestPath + 1, path.end(), std::back_inserter(result));
return result; return static_cast<std::size_t>(newEnd - begin);
} }
// This function checks if the path has a small U-turn, that is, std::size_t fixupShortcuts(dtPolyRef* path, std::size_t pathSize, const dtNavMeshQuery& navQuery)
// a polygon further in the path is adjacent to the first polygon
// in the path. If that happens, a shortcut is taken.
// This can happen if the target (T) location is at tile boundary,
// and we're (S) approaching it parallel to the tile edge.
// The choice at the vertex can be arbitrary,
// +---+---+
// |:::|:::|
// +-S-+-T-+
// |:::| | <-- the step can end up in here, resulting U-turn path.
// +---+---+
std::vector<dtPolyRef> fixupShortcuts(const std::vector<dtPolyRef>& path, const dtNavMeshQuery& navQuery)
{ {
if (path.size() < 3) if (pathSize < 3)
return path; return pathSize;
// Get connected polygons // Get connected polygons
const dtMeshTile* tile = nullptr; const dtMeshTile* tile = nullptr;
const dtPoly* poly = nullptr; const dtPoly* poly = nullptr;
if (dtStatusFailed(navQuery.getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly))) if (dtStatusFailed(navQuery.getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly)))
return path; return pathSize;
const std::size_t maxNeis = 16; const std::size_t maxNeis = 16;
std::array<dtPolyRef, maxNeis> neis; std::array<dtPolyRef, maxNeis> neis;
@ -83,7 +73,7 @@ namespace DetourNavigator
// in the path, short cut to that polygon directly. // in the path, short cut to that polygon directly.
const std::size_t maxLookAhead = 6; const std::size_t maxLookAhead = 6;
std::size_t cut = 0; std::size_t cut = 0;
for (std::size_t i = std::min(maxLookAhead, path.size()) - 1; i > 1 && cut == 0; i--) for (std::size_t i = std::min(maxLookAhead, pathSize) - 1; i > 1 && cut == 0; i--)
{ {
for (std::size_t j = 0; j < nneis; j++) for (std::size_t j = 0; j < nneis; j++)
{ {
@ -95,18 +85,15 @@ namespace DetourNavigator
} }
} }
if (cut <= 1) if (cut <= 1)
return path; return pathSize;
std::vector<dtPolyRef> result; const std::ptrdiff_t offset = static_cast<std::ptrdiff_t>(cut) - 1;
const auto offset = cut - 1; std::copy(path + offset, path + pathSize, path);
result.reserve(1 + path.size() - offset); return pathSize - offset;
result.push_back(path.front());
std::copy(path.begin() + std::ptrdiff_t(offset), path.end(), std::back_inserter(result));
return result;
} }
std::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& startPos, std::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& startPos,
const osg::Vec3f& endPos, const float minTargetDist, const std::vector<dtPolyRef>& path) const osg::Vec3f& endPos, const float minTargetDist, const dtPolyRef* path, const std::size_t pathSize)
{ {
// Find steer target. // Find steer target.
SteerTarget result; SteerTarget result;
@ -115,8 +102,8 @@ namespace DetourNavigator
std::array<unsigned char, maxSteerPoints> steerPathFlags; std::array<unsigned char, maxSteerPoints> steerPathFlags;
std::array<dtPolyRef, maxSteerPoints> steerPathPolys; std::array<dtPolyRef, maxSteerPoints> steerPathPolys;
int nsteerPath = 0; int nsteerPath = 0;
const dtStatus status = navMeshQuery.findStraightPath(startPos.ptr(), endPos.ptr(), path.data(), const dtStatus status = navMeshQuery.findStraightPath(startPos.ptr(), endPos.ptr(), path,
static_cast<int>(path.size()), steerPath.data(), steerPathFlags.data(), steerPathPolys.data(), static_cast<int>(pathSize), steerPath.data(), steerPathFlags.data(), steerPathPolys.data(),
&nsteerPath, maxSteerPoints); &nsteerPath, maxSteerPoints);
if (dtStatusFailed(status)) if (dtStatusFailed(status))
return std::nullopt; return std::nullopt;
@ -138,10 +125,10 @@ namespace DetourNavigator
if (ns >= static_cast<std::size_t>(nsteerPath)) if (ns >= static_cast<std::size_t>(nsteerPath))
return std::nullopt; return std::nullopt;
dtVcopy(result.steerPos.ptr(), &steerPath[ns * 3]); dtVcopy(result.mSteerPos.ptr(), &steerPath[ns * 3]);
result.steerPos.y() = startPos[1]; result.mSteerPos.y() = startPos[1];
result.steerPosFlag = steerPathFlags[ns]; result.mSteerPosFlag = steerPathFlags[ns];
result.steerPosRef = steerPathPolys[ns]; result.mSteerPosRef = steerPathPolys[ns];
return result; return result;
} }

52
components/detournavigator/findsmoothpath.hpp
View File

@ -30,7 +30,7 @@ namespace DetourNavigator
return (osg::Vec2f(v1.x(), v1.z()) - osg::Vec2f(v2.x(), v2.z())).length() < r; return (osg::Vec2f(v1.x(), v1.z()) - osg::Vec2f(v2.x(), v2.z())).length() < r;
} }
std::vector<dtPolyRef> fixupCorridor(const std::vector<dtPolyRef>& path, const std::vector<dtPolyRef>& visited); std::size_t fixupCorridor(dtPolyRef* path, std::size_t pathSize, const std::vector<dtPolyRef>& visited);
// This function checks if the path has a small U-turn, that is, // This function checks if the path has a small U-turn, that is,
// a polygon further in the path is adjacent to the first polygon // a polygon further in the path is adjacent to the first polygon
@ -43,17 +43,17 @@ namespace DetourNavigator
// +-S-+-T-+ // +-S-+-T-+
// |:::| | <-- the step can end up in here, resulting U-turn path. // |:::| | <-- the step can end up in here, resulting U-turn path.
// +---+---+ // +---+---+
std::vector<dtPolyRef> fixupShortcuts(const std::vector<dtPolyRef>& path, const dtNavMeshQuery& navQuery); std::size_t fixupShortcuts(dtPolyRef* path, std::size_t pathSize, const dtNavMeshQuery& navQuery);
struct SteerTarget struct SteerTarget
{ {
osg::Vec3f steerPos; osg::Vec3f mSteerPos;
unsigned char steerPosFlag; unsigned char mSteerPosFlag;
dtPolyRef steerPosRef; dtPolyRef mSteerPosRef;
}; };
std::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos, std::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos,
const osg::Vec3f& endPos, const float minTargetDist, const std::vector<dtPolyRef>& path); const osg::Vec3f& endPos, const float minTargetDist, const dtPolyRef* path, const std::size_t pathSize);
template <class OutputIterator> template <class OutputIterator>
class OutputTransformIterator class OutputTransformIterator
@ -158,22 +158,23 @@ namespace DetourNavigator
*out++ = iterPos; *out++ = iterPos;
std::size_t smoothPathSize = 1; std::size_t smoothPathSize = 1;
std::size_t polygonPathSize = polygonPath.size();
// Move towards target a small advancement at a time until target reached or // Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path. // when ran out of memory to store the path.
while (!polygonPath.empty() && smoothPathSize < maxSmoothPathSize) while (polygonPathSize > 0 && smoothPathSize < maxSmoothPathSize)
{ {
// Find location to steer towards. // Find location to steer towards.
const auto steerTarget = getSteerTarget(navMeshQuery, iterPos, targetPos, slop, polygonPath); const auto steerTarget = getSteerTarget(navMeshQuery, iterPos, targetPos, slop, polygonPath.data(), polygonPathSize);
if (!steerTarget) if (!steerTarget)
break; break;
const bool endOfPath = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_END); const bool endOfPath = bool(steerTarget->mSteerPosFlag & DT_STRAIGHTPATH_END);
const bool offMeshConnection = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION); const bool offMeshConnection = bool(steerTarget->mSteerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
// Find movement delta. // Find movement delta.
const osg::Vec3f delta = steerTarget->steerPos - iterPos; const osg::Vec3f delta = steerTarget->mSteerPos - iterPos;
float len = delta.length(); float len = delta.length();
// If the steer target is end of path or off-mesh link, do not move past the location. // If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < stepSize) if ((endOfPath || offMeshConnection) && len < stepSize)
@ -187,11 +188,11 @@ namespace DetourNavigator
if (!result) if (!result)
return Status::MoveAlongSurfaceFailed; return Status::MoveAlongSurfaceFailed;
polygonPath = fixupCorridor(polygonPath, result->mVisited); polygonPathSize = fixupCorridor(polygonPath.data(), polygonPathSize, result->mVisited);
polygonPath = fixupShortcuts(polygonPath, navMeshQuery); polygonPathSize = fixupShortcuts(polygonPath.data(), polygonPathSize, navMeshQuery);
// Handle end of path and off-mesh links when close enough. // Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(result->mResultPos, steerTarget->steerPos, slop)) if (endOfPath && inRange(result->mResultPos, steerTarget->mSteerPos, slop))
{ {
// Reached end of path. // Reached end of path.
iterPos = targetPos; iterPos = targetPos;
@ -199,20 +200,26 @@ namespace DetourNavigator
++smoothPathSize; ++smoothPathSize;
break; break;
} }
else if (offMeshConnection && inRange(result->mResultPos, steerTarget->steerPos, slop))
dtPolyRef polyRef = polygonPath.front();
osg::Vec3f polyPos = result->mResultPos;
if (offMeshConnection && inRange(polyPos, steerTarget->mSteerPos, slop))
{ {
// Advance the path up to and over the off-mesh connection. // Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0; dtPolyRef prevRef = 0;
dtPolyRef polyRef = polygonPath.front();
std::size_t npos = 0; std::size_t npos = 0;
while (npos < polygonPath.size() && polyRef != steerTarget->steerPosRef) while (npos < polygonPathSize && polyRef != steerTarget->mSteerPosRef)
{ {
prevRef = polyRef; prevRef = polyRef;
polyRef = polygonPath[npos]; polyRef = polygonPath[npos];
++npos; ++npos;
} }
std::copy(polygonPath.begin() + std::ptrdiff_t(npos), polygonPath.end(), polygonPath.begin()); if (npos > 0)
polygonPath.resize(polygonPath.size() - npos); {
std::copy(polygonPath.begin() + npos, polygonPath.begin() + polygonPathSize, polygonPath.begin());
polygonPathSize -= npos;
}
// Reached off-mesh connection. // Reached off-mesh connection.
osg::Vec3f startPos; osg::Vec3f startPos;
@ -233,14 +240,11 @@ namespace DetourNavigator
} }
// Move position at the other side of the off-mesh link. // Move position at the other side of the off-mesh link.
if (dtStatusFailed(navMeshQuery.getPolyHeight(polygonPath.front(), endPos.ptr(), &iterPos.y()))) polyPos = endPos;
return Status::GetPolyHeightFailed;
iterPos.x() = endPos.x();
iterPos.z() = endPos.z();
} }
} }
if (dtStatusFailed(navMeshQuery.getPolyHeight(polygonPath.front(), result->mResultPos.ptr(), &iterPos.y()))) if (dtStatusFailed(navMeshQuery.getPolyHeight(polyRef, polyPos.ptr(), &iterPos.y())))
return Status::GetPolyHeightFailed; return Status::GetPolyHeightFailed;
iterPos.x() = result->mResultPos.x(); iterPos.x() = result->mResultPos.x();
iterPos.z() = result->mResultPos.z(); iterPos.z() = result->mResultPos.z();