diff --git a/components/detournavigator/findsmoothpath.cpp b/components/detournavigator/findsmoothpath.cpp index 14fe696f10..e166ff90e9 100644 --- a/components/detournavigator/findsmoothpath.cpp +++ b/components/detournavigator/findsmoothpath.cpp @@ -7,12 +7,16 @@ namespace DetourNavigator { - std::vector fixupCorridor(const std::vector& path, const std::vector& visited) + std::size_t fixupCorridor(dtPolyRef* path, std::size_t pathSize, const std::vector& visited) { std::vector::const_reverse_iterator furthestVisited; // 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); if (it == visited.rend()) @@ -22,8 +26,8 @@ namespace DetourNavigator }); // If no intersection found just return current path. - if (it == path.rend()) - return path; + if (it == rend) + return pathSize; const auto furthestPath = it.base() - 1; // Concatenate paths. @@ -34,36 +38,22 @@ namespace DetourNavigator // ^ furthestPath // result: x b_n ... b_1 D - std::vector result; - result.reserve(static_cast(furthestVisited - visited.rbegin()) - + static_cast(path.end() - furthestPath) - 1); - std::copy(visited.rbegin(), furthestVisited + 1, std::back_inserter(result)); - std::copy(furthestPath + 1, path.end(), std::back_inserter(result)); + auto newEnd = std::copy(visited.rbegin(), furthestVisited + 1, begin); + newEnd = std::copy(furthestPath + 1, end, newEnd); - return result; + return static_cast(newEnd - begin); } - // 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 - // 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 fixupShortcuts(const std::vector& path, const dtNavMeshQuery& navQuery) + std::size_t fixupShortcuts(dtPolyRef* path, std::size_t pathSize, const dtNavMeshQuery& navQuery) { - if (path.size() < 3) - return path; + if (pathSize < 3) + return pathSize; // Get connected polygons const dtMeshTile* tile = nullptr; const dtPoly* poly = nullptr; if (dtStatusFailed(navQuery.getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly))) - return path; + return pathSize; const std::size_t maxNeis = 16; std::array neis; @@ -83,7 +73,7 @@ namespace DetourNavigator // in the path, short cut to that polygon directly. const std::size_t maxLookAhead = 6; 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++) { @@ -95,18 +85,15 @@ namespace DetourNavigator } } if (cut <= 1) - return path; + return pathSize; - std::vector result; - const auto offset = cut - 1; - result.reserve(1 + path.size() - offset); - result.push_back(path.front()); - std::copy(path.begin() + std::ptrdiff_t(offset), path.end(), std::back_inserter(result)); - return result; + const std::ptrdiff_t offset = static_cast(cut) - 1; + std::copy(path + offset, path + pathSize, path); + return pathSize - offset; } std::optional getSteerTarget(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& startPos, - const osg::Vec3f& endPos, const float minTargetDist, const std::vector& path) + const osg::Vec3f& endPos, const float minTargetDist, const dtPolyRef* path, const std::size_t pathSize) { // Find steer target. SteerTarget result; @@ -115,8 +102,8 @@ namespace DetourNavigator std::array steerPathFlags; std::array steerPathPolys; int nsteerPath = 0; - const dtStatus status = navMeshQuery.findStraightPath(startPos.ptr(), endPos.ptr(), path.data(), - static_cast(path.size()), steerPath.data(), steerPathFlags.data(), steerPathPolys.data(), + const dtStatus status = navMeshQuery.findStraightPath(startPos.ptr(), endPos.ptr(), path, + static_cast(pathSize), steerPath.data(), steerPathFlags.data(), steerPathPolys.data(), &nsteerPath, maxSteerPoints); if (dtStatusFailed(status)) return std::nullopt; @@ -138,10 +125,10 @@ namespace DetourNavigator if (ns >= static_cast(nsteerPath)) return std::nullopt; - dtVcopy(result.steerPos.ptr(), &steerPath[ns * 3]); - result.steerPos.y() = startPos[1]; - result.steerPosFlag = steerPathFlags[ns]; - result.steerPosRef = steerPathPolys[ns]; + dtVcopy(result.mSteerPos.ptr(), &steerPath[ns * 3]); + result.mSteerPos.y() = startPos[1]; + result.mSteerPosFlag = steerPathFlags[ns]; + result.mSteerPosRef = steerPathPolys[ns]; return result; } diff --git a/components/detournavigator/findsmoothpath.hpp b/components/detournavigator/findsmoothpath.hpp index 8eb0bf9f34..71e8a1df88 100644 --- a/components/detournavigator/findsmoothpath.hpp +++ b/components/detournavigator/findsmoothpath.hpp @@ -30,7 +30,7 @@ namespace DetourNavigator return (osg::Vec2f(v1.x(), v1.z()) - osg::Vec2f(v2.x(), v2.z())).length() < r; } - std::vector fixupCorridor(const std::vector& path, const std::vector& visited); + std::size_t fixupCorridor(dtPolyRef* path, std::size_t pathSize, const std::vector& visited); // 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 @@ -43,17 +43,17 @@ namespace DetourNavigator // +-S-+-T-+ // |:::| | <-- the step can end up in here, resulting U-turn path. // +---+---+ - std::vector fixupShortcuts(const std::vector& path, const dtNavMeshQuery& navQuery); + std::size_t fixupShortcuts(dtPolyRef* path, std::size_t pathSize, const dtNavMeshQuery& navQuery); struct SteerTarget { - osg::Vec3f steerPos; - unsigned char steerPosFlag; - dtPolyRef steerPosRef; + osg::Vec3f mSteerPos; + unsigned char mSteerPosFlag; + dtPolyRef mSteerPosRef; }; std::optional getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos, - const osg::Vec3f& endPos, const float minTargetDist, const std::vector& path); + const osg::Vec3f& endPos, const float minTargetDist, const dtPolyRef* path, const std::size_t pathSize); template class OutputTransformIterator @@ -158,22 +158,23 @@ namespace DetourNavigator *out++ = iterPos; std::size_t smoothPathSize = 1; + std::size_t polygonPathSize = polygonPath.size(); // Move towards target a small advancement at a time until target reached or // when ran out of memory to store the path. - while (!polygonPath.empty() && smoothPathSize < maxSmoothPathSize) + while (polygonPathSize > 0 && smoothPathSize < maxSmoothPathSize) { // 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) break; - const bool endOfPath = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_END); - const bool offMeshConnection = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION); + const bool endOfPath = bool(steerTarget->mSteerPosFlag & DT_STRAIGHTPATH_END); + const bool offMeshConnection = bool(steerTarget->mSteerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION); // Find movement delta. - const osg::Vec3f delta = steerTarget->steerPos - iterPos; + const osg::Vec3f delta = steerTarget->mSteerPos - 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 < stepSize) @@ -187,11 +188,11 @@ namespace DetourNavigator if (!result) return Status::MoveAlongSurfaceFailed; - polygonPath = fixupCorridor(polygonPath, result->mVisited); - polygonPath = fixupShortcuts(polygonPath, navMeshQuery); + polygonPathSize = fixupCorridor(polygonPath.data(), polygonPathSize, result->mVisited); + polygonPathSize = fixupShortcuts(polygonPath.data(), polygonPathSize, navMeshQuery); // 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. iterPos = targetPos; @@ -199,20 +200,26 @@ namespace DetourNavigator ++smoothPathSize; 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. dtPolyRef prevRef = 0; - dtPolyRef polyRef = polygonPath.front(); std::size_t npos = 0; - while (npos < polygonPath.size() && polyRef != steerTarget->steerPosRef) + while (npos < polygonPathSize && polyRef != steerTarget->mSteerPosRef) { prevRef = polyRef; polyRef = polygonPath[npos]; ++npos; } - std::copy(polygonPath.begin() + std::ptrdiff_t(npos), polygonPath.end(), polygonPath.begin()); - polygonPath.resize(polygonPath.size() - npos); + if (npos > 0) + { + std::copy(polygonPath.begin() + npos, polygonPath.begin() + polygonPathSize, polygonPath.begin()); + polygonPathSize -= npos; + } // Reached off-mesh connection. osg::Vec3f startPos; @@ -233,14 +240,11 @@ namespace DetourNavigator } // Move position at the other side of the off-mesh link. - if (dtStatusFailed(navMeshQuery.getPolyHeight(polygonPath.front(), endPos.ptr(), &iterPos.y()))) - return Status::GetPolyHeightFailed; - iterPos.x() = endPos.x(); - iterPos.z() = endPos.z(); + polyPos = endPos; } } - if (dtStatusFailed(navMeshQuery.getPolyHeight(polygonPath.front(), result->mResultPos.ptr(), &iterPos.y()))) + if (dtStatusFailed(navMeshQuery.getPolyHeight(polyRef, polyPos.ptr(), &iterPos.y()))) return Status::GetPolyHeightFailed; iterPos.x() = result->mResultPos.x(); iterPos.z() = result->mResultPos.z();