diff --git a/components/detournavigator/dtstatus.hpp b/components/detournavigator/dtstatus.hpp index 5360b5ce3..a73d33be1 100644 --- a/components/detournavigator/dtstatus.hpp +++ b/components/detournavigator/dtstatus.hpp @@ -1,8 +1,6 @@ #ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H #define OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H -#include "exceptions.hpp" - #include #include @@ -35,32 +33,6 @@ namespace DetourNavigator stream << status.second << " "; return stream; } - - inline void checkDtStatus(dtStatus status, const char* call, int line) - { - if (!dtStatusSucceed(status)) - { - std::ostringstream message; - message << call << " failed with status=" << WriteDtStatus {status} << " at " __FILE__ ":" << line; - throw NavigatorException(message.str()); - } - } - - inline void checkDtResult(bool result, const char* call, int line) - { - if (!result) - { - std::ostringstream message; - message << call << " failed at " __FILE__ ":" << line; - throw NavigatorException(message.str()); - } - } } -#define OPENMW_CHECK_DT_STATUS(call) \ - do { DetourNavigator::checkDtStatus((call), #call, __LINE__); } while (false) - -#define OPENMW_CHECK_DT_RESULT(call) \ - do { DetourNavigator::checkDtResult((call), #call, __LINE__); } while (false) - #endif diff --git a/components/detournavigator/findsmoothpath.hpp b/components/detournavigator/findsmoothpath.hpp index 08bfa14da..81b732b74 100644 --- a/components/detournavigator/findsmoothpath.hpp +++ b/components/detournavigator/findsmoothpath.hpp @@ -6,6 +6,7 @@ #include "flags.hpp" #include "settings.hpp" #include "settingsutils.hpp" +#include "debug.hpp" #include #include @@ -97,6 +98,73 @@ namespace DetourNavigator const Settings& mSettings; }; + inline void initNavMeshQuery(dtNavMeshQuery& value, const dtNavMesh& navMesh, const int maxNodes) + { + const auto status = value.init(&navMesh, maxNodes); + if (!dtStatusSucceed(status)) + throw NavigatorException("Failed to init navmesh query"); + } + + struct MoveAlongSurfaceResult + { + osg::Vec3f mResultPos; + std::vector mVisited; + }; + + inline MoveAlongSurfaceResult moveAlongSurface(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef, + const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& filter, + const std::size_t maxVisitedSize) + { + MoveAlongSurfaceResult result; + result.mVisited.resize(maxVisitedSize); + int visitedNumber = 0; + const auto status = navMeshQuery.moveAlongSurface(startRef, startPos.ptr(), endPos.ptr(), + &filter, result.mResultPos.ptr(), result.mVisited.data(), &visitedNumber, static_cast(maxVisitedSize)); + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << "Failed to move along surface from " << startPos << " to " << endPos; + throw NavigatorException(message.str()); + } + assert(visitedNumber >= 0); + assert(visitedNumber <= static_cast(maxVisitedSize)); + result.mVisited.resize(static_cast(visitedNumber)); + return result; + } + + inline std::vector 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) + { + int pathLen = 0; + std::vector result(maxSize); + const auto status = navMeshQuery.findPath(startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter, + result.data(), &pathLen, static_cast(maxSize)); + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << "Failed to find path over polygons from " << startRef << " to " << endRef; + throw NavigatorException(message.str()); + } + assert(pathLen >= 0); + assert(static_cast(pathLen) <= maxSize); + result.resize(static_cast(pathLen)); + return result; + } + + inline float getPolyHeight(const dtNavMeshQuery& navMeshQuery, const dtPolyRef ref, const osg::Vec3f& pos) + { + float result = 0.0f; + const auto status = navMeshQuery.getPolyHeight(ref, pos.ptr(), &result); + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << "Failed to get polygon height ref=" << ref << " pos=" << pos; + throw NavigatorException(message.str()); + } + return result; + } + template OutputIterator makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end, @@ -139,25 +207,15 @@ namespace DetourNavigator len = STEP_SIZE / len; const osg::Vec3f moveTgt = iterPos + delta * len; + const auto result = moveAlongSurface(navMeshQuery, polygonPath.front(), iterPos, moveTgt, filter, 16); - // Move - osg::Vec3f result; - std::vector visited(16); - int nvisited = 0; - OPENMW_CHECK_DT_STATUS(navMeshQuery.moveAlongSurface(polygonPath.front(), iterPos.ptr(), moveTgt.ptr(), - &filter, result.ptr(), visited.data(), &nvisited, int(visited.size()))); - - assert(nvisited >= 0); - assert(nvisited <= int(visited.size())); - visited.resize(static_cast(nvisited)); - - polygonPath = fixupCorridor(polygonPath, visited); + polygonPath = fixupCorridor(polygonPath, result.mVisited); polygonPath = fixupShortcuts(polygonPath, navMeshQuery); float h = 0; - navMeshQuery.getPolyHeight(polygonPath.front(), result.ptr(), &h); - result.y() = h; - iterPos = result; + navMeshQuery.getPolyHeight(polygonPath.front(), result.mResultPos.ptr(), &h); + iterPos = result.mResultPos; + iterPos.y() = h; // Handle end of path and off-mesh links when close enough. if (endOfPath && inRange(iterPos, steerTarget->steerPos, SLOP, 1.0f)) @@ -203,9 +261,7 @@ namespace DetourNavigator // Move position at the other side of the off-mesh link. iterPos = endPos; - float eh = 0.0f; - OPENMW_CHECK_DT_STATUS(navMeshQuery.getPolyHeight(polygonPath.front(), iterPos.ptr(), &eh)); - iterPos.y() = eh; + iterPos.y() = getPolyHeight(navMeshQuery, polygonPath.front(), iterPos); } } @@ -223,7 +279,7 @@ namespace DetourNavigator const Settings& settings, OutputIterator out) { dtNavMeshQuery navMeshQuery; - OPENMW_CHECK_DT_STATUS(navMeshQuery.init(&navMesh, settings.mMaxNavMeshQueryNodes)); + initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes); dtQueryFilter queryFilter; queryFilter.setIncludeFlags(includeFlags); @@ -239,7 +295,7 @@ namespace DetourNavigator } if (startRef == 0) - throw NavigatorException("start polygon is not found at " __FILE__ ":" + std::to_string(__LINE__)); + throw NavigatorException("Navmesh polygon for start point is not found"); dtPolyRef endRef = 0; osg::Vec3f endPolygonPosition; @@ -252,16 +308,10 @@ namespace DetourNavigator } if (endRef == 0) - throw NavigatorException("end polygon is not found at " __FILE__ ":" + std::to_string(__LINE__)); - - std::vector polygonPath(settings.mMaxPolygonPathSize); - int pathLen = 0; - OPENMW_CHECK_DT_STATUS(navMeshQuery.findPath(startRef, endRef, start.ptr(), end.ptr(), &queryFilter, - polygonPath.data(), &pathLen, static_cast(polygonPath.size()))); - - assert(pathLen >= 0); + throw NavigatorException("Navmesh polygon for end polygon is not found"); - polygonPath.resize(static_cast(pathLen)); + const auto polygonPath = findPath(navMeshQuery, startRef, endRef, start, end, queryFilter, + settings.mMaxPolygonPathSize); if (polygonPath.empty() || polygonPath.back() != endRef) return out; diff --git a/components/detournavigator/makenavmesh.cpp b/components/detournavigator/makenavmesh.cpp index cf6827b64..31ea64f18 100644 --- a/components/detournavigator/makenavmesh.cpp +++ b/components/detournavigator/makenavmesh.cpp @@ -102,6 +102,77 @@ namespace return result; } + void createHeightfield(rcContext& context, rcHeightfield& solid, int width, int height, const float* bmin, + const float* bmax, const float cs, const float ch) + { + const auto result = rcCreateHeightfield(&context, solid, width, height, bmin, bmax, cs, ch); + + if (!result) + throw NavigatorException("Failed to create heightfield for navmesh"); + } + + void buildCompactHeightfield(rcContext& context, const int walkableHeight, const int walkableClimb, + rcHeightfield& solid, rcCompactHeightfield& compact) + { + const auto result = rcBuildCompactHeightfield(&context, walkableHeight, + walkableClimb, solid, compact); + + if (!result) + throw NavigatorException("Failed to build compact heightfield for navmesh"); + } + + void erodeWalkableArea(rcContext& context, int walkableRadius, rcCompactHeightfield& compact) + { + const auto result = rcErodeWalkableArea(&context, walkableRadius, compact); + + if (!result) + throw NavigatorException("Failed to erode walkable area for navmesh"); + } + + void buildDistanceField(rcContext& context, rcCompactHeightfield& compact) + { + const auto result = rcBuildDistanceField(&context, compact); + + if (!result) + throw NavigatorException("Failed to build distance field for navmesh"); + } + + void buildRegions(rcContext& context, rcCompactHeightfield& compact, const int borderSize, + const int minRegionArea, const int mergeRegionArea) + { + const auto result = rcBuildRegions(&context, compact, borderSize, minRegionArea, mergeRegionArea); + + if (!result) + throw NavigatorException("Failed to build distance field for navmesh"); + } + + void buildContours(rcContext& context, rcCompactHeightfield& compact, const float maxError, const int maxEdgeLen, + rcContourSet& contourSet, const int buildFlags = RC_CONTOUR_TESS_WALL_EDGES) + { + const auto result = rcBuildContours(&context, compact, maxError, maxEdgeLen, contourSet, buildFlags); + + if (!result) + throw NavigatorException("Failed to build contours for navmesh"); + } + + void buildPolyMesh(rcContext& context, rcContourSet& contourSet, const int maxVertsPerPoly, rcPolyMesh& polyMesh) + { + const auto result = rcBuildPolyMesh(&context, contourSet, maxVertsPerPoly, polyMesh); + + if (!result) + throw NavigatorException("Failed to build poly mesh for navmesh"); + } + + void buildPolyMeshDetail(rcContext& context, const rcPolyMesh& polyMesh, const rcCompactHeightfield& compact, + const float sampleDist, const float sampleMaxError, rcPolyMeshDetail& polyMeshDetail) + { + const auto result = rcBuildPolyMeshDetail(&context, polyMesh, compact, sampleDist, sampleMaxError, + polyMeshDetail); + + if (!result) + throw NavigatorException("Failed to build detail poly mesh for navmesh"); + } + NavMeshData makeNavMeshTileData(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, const std::vector& offMeshConnections, const int tileX, const int tileY, const osg::Vec3f& boundsMin, const osg::Vec3f& boundsMax, const Settings& settings) @@ -133,8 +204,7 @@ namespace config.bmax[2] += getBorderSize(settings); rcHeightfield solid; - OPENMW_CHECK_DT_RESULT(rcCreateHeightfield(nullptr, solid, config.width, config.height, - config.bmin, config.bmax, config.cs, config.ch)); + createHeightfield(context, solid, config.width, config.height, config.bmin, config.bmax, config.cs, config.ch); { const auto& chunkyMesh = recastMesh.getChunkyTriMesh(); @@ -181,7 +251,7 @@ namespace areas.data() ); - OPENMW_CHECK_DT_RESULT(rcRasterizeTriangles( + const auto trianglesRasterized = rcRasterizeTriangles( &context, recastMesh.getVertices().data(), static_cast(recastMesh.getVerticesCount()), @@ -190,7 +260,10 @@ namespace static_cast(chunk.mSize), solid, config.walkableClimb - )); + ); + + if (!trianglesRasterized) + throw NavigatorException("Failed to create rasterize triangles from recast mesh for navmesh"); } } @@ -231,7 +304,7 @@ namespace 0, 2, 3, }}; - OPENMW_CHECK_DT_RESULT(rcRasterizeTriangles( + const auto trianglesRasterized = rcRasterizeTriangles( &context, convertedVertices.data(), static_cast(convertedVertices.size() / 3), @@ -240,7 +313,10 @@ namespace static_cast(areas.size()), solid, config.walkableClimb - )); + ); + + if (!trianglesRasterized) + throw NavigatorException("Failed to create rasterize water triangles for navmesh"); } } @@ -254,24 +330,22 @@ namespace const PolyMeshDetailStackPtr polyMeshDetailPtr(&polyMeshDetail); { rcCompactHeightfield compact; + buildCompactHeightfield(context, config.walkableHeight, config.walkableClimb, solid, compact); - OPENMW_CHECK_DT_RESULT(rcBuildCompactHeightfield(&context, config.walkableHeight, config.walkableClimb, - solid, compact)); - OPENMW_CHECK_DT_RESULT(rcErodeWalkableArea(&context, config.walkableRadius, compact)); - OPENMW_CHECK_DT_RESULT(rcBuildDistanceField(&context, compact)); - OPENMW_CHECK_DT_RESULT(rcBuildRegions(&context, compact, config.borderSize, config.minRegionArea, - config.mergeRegionArea)); + erodeWalkableArea(context, config.walkableRadius, compact); + buildDistanceField(context, compact); + buildRegions(context, compact, config.borderSize, config.minRegionArea, config.mergeRegionArea); rcContourSet contourSet; - OPENMW_CHECK_DT_RESULT(rcBuildContours(&context, compact, config.maxSimplificationError, config.maxEdgeLen, - contourSet)); + buildContours(context, compact, config.maxSimplificationError, config.maxEdgeLen, contourSet); if (contourSet.nconts == 0) return NavMeshData(); - OPENMW_CHECK_DT_RESULT(rcBuildPolyMesh(&context, contourSet, config.maxVertsPerPoly, polyMesh)); - OPENMW_CHECK_DT_RESULT(rcBuildPolyMeshDetail(&context, polyMesh, compact, config.detailSampleDist, - config.detailSampleMaxError, polyMeshDetail)); + buildPolyMesh(context, contourSet, config.maxVertsPerPoly, polyMesh); + + buildPolyMeshDetail(context, polyMesh, compact, config.detailSampleDist, config.detailSampleMaxError, + polyMeshDetail); } for (int i = 0; i < polyMesh.npolys; ++i) @@ -323,7 +397,10 @@ namespace unsigned char* navMeshData; int navMeshDataSize; - OPENMW_CHECK_DT_RESULT(dtCreateNavMeshData(¶ms, &navMeshData, &navMeshDataSize)); + const auto navMeshDataCreated = dtCreateNavMeshData(¶ms, &navMeshData, &navMeshDataSize); + + if (!navMeshDataCreated) + throw NavigatorException("Failed to create navmesh tile data"); return NavMeshData(navMeshData, navMeshDataSize); } @@ -360,7 +437,10 @@ namespace DetourNavigator params.maxPolys = maxPolysPerTile; NavMeshPtr navMesh(dtAllocNavMesh(), &dtFreeNavMesh); - OPENMW_CHECK_DT_STATUS(navMesh->init(¶ms)); + const auto status = navMesh->init(¶ms); + + if (!dtStatusSucceed(status)) + throw NavigatorException("Failed to init navmesh"); return navMesh; } diff --git a/components/detournavigator/recastmesh.cpp b/components/detournavigator/recastmesh.cpp index 50a3493f7..f5c455977 100644 --- a/components/detournavigator/recastmesh.cpp +++ b/components/detournavigator/recastmesh.cpp @@ -14,7 +14,7 @@ namespace DetourNavigator , mChunkyTriMesh(mVertices, mIndices, mAreaTypes, trianglesPerChunk) { if (getTrianglesCount() != mAreaTypes.size()) - throw InvalidArgument("number of flags doesn't match number of triangles: triangles=" + throw InvalidArgument("Number of flags doesn't match number of triangles: triangles=" + std::to_string(getTrianglesCount()) + ", areaTypes=" + std::to_string(mAreaTypes.size())); if (getVerticesCount()) rcCalcBounds(mVertices.data(), static_cast(getVerticesCount()), mBoundsMin.ptr(), mBoundsMax.ptr());