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