1 #ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H 2 #define OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H 3 4 #include "dtstatus.hpp" 5 #include "exceptions.hpp" 6 #include "flags.hpp" 7 #include "settings.hpp" 8 #include "settingsutils.hpp" 9 #include "debug.hpp" 10 #include "status.hpp" 11 #include "areatype.hpp" 12 13 #include <DetourCommon.h> 14 #include <DetourNavMesh.h> 15 #include <DetourNavMeshQuery.h> 16 17 #include <osg/Vec3f> 18 19 #include <cassert> 20 #include <vector> 21 22 class dtNavMesh; 23 24 namespace DetourNavigator 25 { 26 struct Settings; 27 inRange(const osg::Vec3f & v1,const osg::Vec3f & v2,const float r)28 inline bool inRange(const osg::Vec3f& v1, const osg::Vec3f& v2, const float r) 29 { 30 return (osg::Vec2f(v1.x(), v1.z()) - osg::Vec2f(v2.x(), v2.z())).length() < r; 31 } 32 33 std::vector<dtPolyRef> fixupCorridor(const std::vector<dtPolyRef>& path, const std::vector<dtPolyRef>& visited); 34 35 // This function checks if the path has a small U-turn, that is, 36 // a polygon further in the path is adjacent to the first polygon 37 // in the path. If that happens, a shortcut is taken. 38 // This can happen if the target (T) location is at tile boundary, 39 // and we're (S) approaching it parallel to the tile edge. 40 // The choice at the vertex can be arbitrary, 41 // +---+---+ 42 // |:::|:::| 43 // +-S-+-T-+ 44 // |:::| | <-- the step can end up in here, resulting U-turn path. 45 // +---+---+ 46 std::vector<dtPolyRef> fixupShortcuts(const std::vector<dtPolyRef>& path, const dtNavMeshQuery& navQuery); 47 48 struct SteerTarget 49 { 50 osg::Vec3f steerPos; 51 unsigned char steerPosFlag; 52 dtPolyRef steerPosRef; 53 }; 54 55 std::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos, 56 const osg::Vec3f& endPos, const float minTargetDist, const std::vector<dtPolyRef>& path); 57 58 template <class OutputIterator> 59 class OutputTransformIterator 60 { 61 public: OutputTransformIterator(OutputIterator & impl,const Settings & settings)62 OutputTransformIterator(OutputIterator& impl, const Settings& settings) 63 : mImpl(impl), mSettings(settings) 64 { 65 } 66 operator *()67 OutputTransformIterator& operator *() 68 { 69 return *this; 70 } 71 operator ++()72 OutputTransformIterator& operator ++() 73 { 74 ++mImpl.get(); 75 return *this; 76 } 77 operator ++(int)78 OutputTransformIterator operator ++(int) 79 { 80 const auto copy = *this; 81 ++(*this); 82 return copy; 83 } 84 operator =(const osg::Vec3f & value)85 OutputTransformIterator& operator =(const osg::Vec3f& value) 86 { 87 *mImpl.get() = fromNavMeshCoordinates(mSettings, value); 88 return *this; 89 } 90 91 private: 92 std::reference_wrapper<OutputIterator> mImpl; 93 std::reference_wrapper<const Settings> mSettings; 94 }; 95 initNavMeshQuery(dtNavMeshQuery & value,const dtNavMesh & navMesh,const int maxNodes)96 inline bool initNavMeshQuery(dtNavMeshQuery& value, const dtNavMesh& navMesh, const int maxNodes) 97 { 98 const auto status = value.init(&navMesh, maxNodes); 99 return dtStatusSucceed(status); 100 } 101 102 dtPolyRef findNearestPolyExpanding(const dtNavMeshQuery& query, const dtQueryFilter& filter, 103 const osg::Vec3f& center, const osg::Vec3f& halfExtents); 104 105 struct MoveAlongSurfaceResult 106 { 107 osg::Vec3f mResultPos; 108 std::vector<dtPolyRef> mVisited; 109 }; 110 moveAlongSurface(const dtNavMeshQuery & navMeshQuery,const dtPolyRef startRef,const osg::Vec3f & startPos,const osg::Vec3f & endPos,const dtQueryFilter & filter,const std::size_t maxVisitedSize)111 inline std::optional<MoveAlongSurfaceResult> moveAlongSurface(const dtNavMeshQuery& navMeshQuery, 112 const dtPolyRef startRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& filter, 113 const std::size_t maxVisitedSize) 114 { 115 MoveAlongSurfaceResult result; 116 result.mVisited.resize(maxVisitedSize); 117 int visitedNumber = 0; 118 const auto status = navMeshQuery.moveAlongSurface(startRef, startPos.ptr(), endPos.ptr(), 119 &filter, result.mResultPos.ptr(), result.mVisited.data(), &visitedNumber, static_cast<int>(maxVisitedSize)); 120 if (!dtStatusSucceed(status)) 121 return {}; 122 assert(visitedNumber >= 0); 123 assert(visitedNumber <= static_cast<int>(maxVisitedSize)); 124 result.mVisited.resize(static_cast<std::size_t>(visitedNumber)); 125 return {std::move(result)}; 126 } 127 findPath(const dtNavMeshQuery & navMeshQuery,const dtPolyRef startRef,const dtPolyRef endRef,const osg::Vec3f & startPos,const osg::Vec3f & endPos,const dtQueryFilter & queryFilter,const std::size_t maxSize)128 inline std::optional<std::vector<dtPolyRef>> findPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef, 129 const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter, 130 const std::size_t maxSize) 131 { 132 int pathLen = 0; 133 std::vector<dtPolyRef> result(maxSize); 134 const auto status = navMeshQuery.findPath(startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter, 135 result.data(), &pathLen, static_cast<int>(maxSize)); 136 if (!dtStatusSucceed(status)) 137 return {}; 138 assert(pathLen >= 0); 139 assert(static_cast<std::size_t>(pathLen) <= maxSize); 140 result.resize(static_cast<std::size_t>(pathLen)); 141 return {std::move(result)}; 142 } 143 144 template <class OutputIterator> makeSmoothPath(const dtNavMesh & navMesh,const dtNavMeshQuery & navMeshQuery,const dtQueryFilter & filter,const osg::Vec3f & start,const osg::Vec3f & end,const float stepSize,std::vector<dtPolyRef> polygonPath,std::size_t maxSmoothPathSize,OutputIterator & out)145 Status makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, 146 const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end, const float stepSize, 147 std::vector<dtPolyRef> polygonPath, std::size_t maxSmoothPathSize, OutputIterator& out) 148 { 149 // Iterate over the path to find smooth path on the detail mesh surface. 150 osg::Vec3f iterPos; 151 navMeshQuery.closestPointOnPoly(polygonPath.front(), start.ptr(), iterPos.ptr(), nullptr); 152 153 osg::Vec3f targetPos; 154 navMeshQuery.closestPointOnPoly(polygonPath.back(), end.ptr(), targetPos.ptr(), nullptr); 155 156 constexpr float slop = 0.01f; 157 158 *out++ = iterPos; 159 160 std::size_t smoothPathSize = 1; 161 162 // Move towards target a small advancement at a time until target reached or 163 // when ran out of memory to store the path. 164 while (!polygonPath.empty() && smoothPathSize < maxSmoothPathSize) 165 { 166 // Find location to steer towards. 167 const auto steerTarget = getSteerTarget(navMeshQuery, iterPos, targetPos, slop, polygonPath); 168 169 if (!steerTarget) 170 break; 171 172 const bool endOfPath = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_END); 173 const bool offMeshConnection = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION); 174 175 // Find movement delta. 176 const osg::Vec3f delta = steerTarget->steerPos - iterPos; 177 float len = delta.length(); 178 // If the steer target is end of path or off-mesh link, do not move past the location. 179 if ((endOfPath || offMeshConnection) && len < stepSize) 180 len = 1; 181 else 182 len = stepSize / len; 183 184 const osg::Vec3f moveTgt = iterPos + delta * len; 185 const auto result = moveAlongSurface(navMeshQuery, polygonPath.front(), iterPos, moveTgt, filter, 16); 186 187 if (!result) 188 return Status::MoveAlongSurfaceFailed; 189 190 polygonPath = fixupCorridor(polygonPath, result->mVisited); 191 polygonPath = fixupShortcuts(polygonPath, navMeshQuery); 192 193 // Handle end of path and off-mesh links when close enough. 194 if (endOfPath && inRange(result->mResultPos, steerTarget->steerPos, slop)) 195 { 196 // Reached end of path. 197 iterPos = targetPos; 198 *out++ = iterPos; 199 ++smoothPathSize; 200 break; 201 } 202 else if (offMeshConnection && inRange(result->mResultPos, steerTarget->steerPos, slop)) 203 { 204 // Advance the path up to and over the off-mesh connection. 205 dtPolyRef prevRef = 0; 206 dtPolyRef polyRef = polygonPath.front(); 207 std::size_t npos = 0; 208 while (npos < polygonPath.size() && polyRef != steerTarget->steerPosRef) 209 { 210 prevRef = polyRef; 211 polyRef = polygonPath[npos]; 212 ++npos; 213 } 214 std::copy(polygonPath.begin() + std::ptrdiff_t(npos), polygonPath.end(), polygonPath.begin()); 215 polygonPath.resize(polygonPath.size() - npos); 216 217 // Reached off-mesh connection. 218 osg::Vec3f startPos; 219 osg::Vec3f endPos; 220 221 // Handle the connection. 222 if (dtStatusSucceed(navMesh.getOffMeshConnectionPolyEndPoints(prevRef, polyRef, 223 startPos.ptr(), endPos.ptr()))) 224 { 225 *out++ = startPos; 226 ++smoothPathSize; 227 228 // Hack to make the dotted path not visible during off-mesh connection. 229 if (smoothPathSize & 1) 230 { 231 *out++ = startPos; 232 ++smoothPathSize; 233 } 234 235 // Move position at the other side of the off-mesh link. 236 if (dtStatusFailed(navMeshQuery.getPolyHeight(polygonPath.front(), endPos.ptr(), &iterPos.y()))) 237 return Status::GetPolyHeightFailed; 238 iterPos.x() = endPos.x(); 239 iterPos.z() = endPos.z(); 240 } 241 } 242 243 if (dtStatusFailed(navMeshQuery.getPolyHeight(polygonPath.front(), result->mResultPos.ptr(), &iterPos.y()))) 244 return Status::GetPolyHeightFailed; 245 iterPos.x() = result->mResultPos.x(); 246 iterPos.z() = result->mResultPos.z(); 247 248 // Store results. 249 *out++ = iterPos; 250 ++smoothPathSize; 251 } 252 253 return Status::Success; 254 } 255 256 template <class OutputIterator> findSmoothPath(const dtNavMesh & navMesh,const osg::Vec3f & halfExtents,const float stepSize,const osg::Vec3f & start,const osg::Vec3f & end,const Flags includeFlags,const AreaCosts & areaCosts,const Settings & settings,OutputIterator & out)257 Status findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, const float stepSize, 258 const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts, 259 const Settings& settings, OutputIterator& out) 260 { 261 dtNavMeshQuery navMeshQuery; 262 if (!initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes)) 263 return Status::InitNavMeshQueryFailed; 264 265 dtQueryFilter queryFilter; 266 queryFilter.setIncludeFlags(includeFlags); 267 queryFilter.setAreaCost(AreaType_water, areaCosts.mWater); 268 queryFilter.setAreaCost(AreaType_door, areaCosts.mDoor); 269 queryFilter.setAreaCost(AreaType_pathgrid, areaCosts.mPathgrid); 270 queryFilter.setAreaCost(AreaType_ground, areaCosts.mGround); 271 272 dtPolyRef startRef = findNearestPolyExpanding(navMeshQuery, queryFilter, start, halfExtents); 273 if (startRef == 0) 274 return Status::StartPolygonNotFound; 275 276 dtPolyRef endRef = findNearestPolyExpanding(navMeshQuery, queryFilter, end, halfExtents); 277 if (endRef == 0) 278 return Status::EndPolygonNotFound; 279 280 const auto polygonPath = findPath(navMeshQuery, startRef, endRef, start, end, queryFilter, 281 settings.mMaxPolygonPathSize); 282 283 if (!polygonPath) 284 return Status::FindPathOverPolygonsFailed; 285 286 if (polygonPath->empty() || polygonPath->back() != endRef) 287 return Status::Success; 288 289 auto outTransform = OutputTransformIterator<OutputIterator>(out, settings); 290 return makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize, std::move(*polygonPath), 291 settings.mMaxSmoothPathSize, outTransform); 292 } 293 } 294 295 #endif 296