From ea404d5f6f91d0a55dfe895d56a0042494094055 Mon Sep 17 00:00:00 2001 From: Sergey Lapin Date: Mon, 13 May 2024 11:38:35 +0300 Subject: [PATCH] recast update --- .../recastnavigation/Recast/Include/Recast.h | 141 ++- .../Recast/Include/RecastAlloc.h | 2 +- .../Recast/Include/RecastAssert.h | 2 +- .../Recast/Source/RecastArea.cpp | 955 ++++++++++-------- .../Recast/Source/RecastAssert.cpp | 2 +- .../Recast/Source/RecastContour.cpp | 6 +- .../Recast/Source/RecastFilter.cpp | 139 +-- .../Recast/Source/RecastMesh.cpp | 15 +- .../Recast/Source/RecastMeshDetail.cpp | 68 +- .../Recast/Source/RecastRasterization.cpp | 81 +- .../Recast/Source/RecastRegion.cpp | 13 +- 11 files changed, 810 insertions(+), 614 deletions(-) diff --git a/thirdparty/recastnavigation/Recast/Include/Recast.h b/thirdparty/recastnavigation/Recast/Include/Recast.h index 9def8fd2ae..5b34ea52b8 100644 --- a/thirdparty/recastnavigation/Recast/Include/Recast.h +++ b/thirdparty/recastnavigation/Recast/Include/Recast.h @@ -321,6 +321,8 @@ struct rcHeightfield float cs; ///< The size of each cell. (On the xz-plane.) float ch; ///< The height of each cell. (The minimum increment along the y-axis.) rcSpan** spans; ///< Heightfield of spans (width*height). + + // memory pool for rcSpan instances. rcSpanPool* pools; ///< Linked list of span pools. rcSpan* freelist; ///< The next free span. @@ -669,7 +671,7 @@ template inline T rcAbs(T a) { return a < 0 ? -a : a; } /// Returns the square of the value. /// @param[in] a The value. /// @return The square of the value. -template inline T rcSqr(T a) { return a*a; } +template inline T rcSqr(T a) { return a * a; } /// Clamps the value to the specified range. /// @param[in] value The value to clamp. @@ -998,23 +1000,22 @@ bool rcRasterizeTriangles(rcContext* context, const float* verts, const unsigned char* triAreaIDs, int numTris, rcHeightfield& heightfield, int flagMergeThreshold = 1); -/// Marks non-walkable spans as walkable if their maximum is within @p walkableClimb of a walkable neighbor. +/// Marks non-walkable spans as walkable if their maximum is within @p walkableClimb of the span below them. /// -/// Allows the formation of walkable regions that will flow over low lying -/// objects such as curbs, and up structures such as stairways. +/// This removes small obstacles and rasterization artifacts that the agent would be able to walk over +/// such as curbs. It also allows agents to move up terraced structures like stairs. /// -/// Two neighboring spans are walkable if: rcAbs(currentSpan.smax - neighborSpan.smax) < waklableClimb +/// Obstacle spans are marked walkable if: obstacleSpan.smax - walkableSpan.smax < walkableClimb /// -/// @warning Will override the effect of #rcFilterLedgeSpans. So if both filters are used, call -/// #rcFilterLedgeSpans after calling this filter. +/// @warning Will override the effect of #rcFilterLedgeSpans. If both filters are used, call #rcFilterLedgeSpans only after applying this filter. /// /// @see rcHeightfield, rcConfig /// /// @ingroup recast -/// @param[in,out] context The build context to use during the operation. +/// @param[in,out] context The build context to use during the operation. /// @param[in] walkableClimb Maximum ledge height that is considered to still be traversable. /// [Limit: >=0] [Units: vx] -/// @param[in,out] heightfield A fully built heightfield. (All spans have been added.) +/// @param[in,out] heightfield A fully built heightfield. (All spans have been added.) void rcFilterLowHangingWalkableObstacles(rcContext* context, int walkableClimb, rcHeightfield& heightfield); /// Marks spans that are ledges as not-walkable. @@ -1037,10 +1038,12 @@ void rcFilterLowHangingWalkableObstacles(rcContext* context, int walkableClimb, /// @param[in,out] heightfield A fully built heightfield. (All spans have been added.) void rcFilterLedgeSpans(rcContext* context, int walkableHeight, int walkableClimb, rcHeightfield& heightfield); -/// Marks walkable spans as not walkable if the clearance above the span is less than the specified height. +/// Marks walkable spans as not walkable if the clearance above the span is less than the specified walkableHeight. /// /// For this filter, the clearance above the span is the distance from the span's -/// maximum to the next higher span's minimum. (Same grid column.) +/// maximum to the minimum of the next higher span in the same column. +/// If there is no higher span in the column, the clearance is computed as the +/// distance from the top of the span to the maximum heightfield height. /// /// @see rcHeightfield, rcConfig /// @ingroup recast @@ -1085,66 +1088,98 @@ int rcGetHeightFieldSpanCount(rcContext* context, const rcHeightfield& heightfie bool rcBuildCompactHeightfield(rcContext* context, int walkableHeight, int walkableClimb, const rcHeightfield& heightfield, rcCompactHeightfield& compactHeightfield); -/// Erodes the walkable area within the heightfield by the specified radius. +/// Erodes the walkable area within the heightfield by the specified radius. +/// +/// Basically, any spans that are closer to a boundary or obstruction than the specified radius +/// are marked as un-walkable. +/// +/// This method is usually called immediately after the heightfield has been built. +/// +/// @see rcCompactHeightfield, rcBuildCompactHeightfield, rcConfig::walkableRadius /// @ingroup recast -/// @param[in,out] ctx The build context to use during the operation. -/// @param[in] radius The radius of erosion. [Limits: 0 < value < 255] [Units: vx] -/// @param[in,out] chf The populated compact heightfield to erode. +/// +/// @param[in,out] context The build context to use during the operation. +/// @param[in] erosionRadius The radius of erosion. [Limits: 0 < value < 255] [Units: vx] +/// @param[in,out] compactHeightfield The populated compact heightfield to erode. /// @returns True if the operation completed successfully. -bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf); +bool rcErodeWalkableArea(rcContext* context, int erosionRadius, rcCompactHeightfield& compactHeightfield); /// Applies a median filter to walkable area types (based on area id), removing noise. +/// +/// This filter is usually applied after applying area id's using functions +/// such as #rcMarkBoxArea, #rcMarkConvexPolyArea, and #rcMarkCylinderArea. +/// +/// @see rcCompactHeightfield /// @ingroup recast -/// @param[in,out] ctx The build context to use during the operation. -/// @param[in,out] chf A populated compact heightfield. +/// +/// @param[in,out] context The build context to use during the operation. +/// @param[in,out] compactHeightfield A populated compact heightfield. /// @returns True if the operation completed successfully. -bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf); +bool rcMedianFilterWalkableArea(rcContext* context, rcCompactHeightfield& compactHeightfield); /// Applies an area id to all spans within the specified bounding box. (AABB) +/// +/// @see rcCompactHeightfield, rcMedianFilterWalkableArea /// @ingroup recast -/// @param[in,out] ctx The build context to use during the operation. -/// @param[in] bmin The minimum of the bounding box. [(x, y, z)] -/// @param[in] bmax The maximum of the bounding box. [(x, y, z)] -/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] -/// @param[in,out] chf A populated compact heightfield. -void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId, - rcCompactHeightfield& chf); +/// +/// @param[in,out] context The build context to use during the operation. +/// @param[in] boxMinBounds The minimum extents of the bounding box. [(x, y, z)] [Units: wu] +/// @param[in] boxMaxBounds The maximum extents of the bounding box. [(x, y, z)] [Units: wu] +/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] +/// @param[in,out] compactHeightfield A populated compact heightfield. +void rcMarkBoxArea(rcContext* context, const float* boxMinBounds, const float* boxMaxBounds, unsigned char areaId, + rcCompactHeightfield& compactHeightfield); /// Applies the area id to the all spans within the specified convex polygon. +/// +/// The value of spacial parameters are in world units. +/// +/// The y-values of the polygon vertices are ignored. So the polygon is effectively +/// projected onto the xz-plane, translated to @p minY, and extruded to @p maxY. +/// +/// @see rcCompactHeightfield, rcMedianFilterWalkableArea /// @ingroup recast -/// @param[in,out] ctx The build context to use during the operation. -/// @param[in] verts The vertices of the polygon [Fomr: (x, y, z) * @p nverts] -/// @param[in] nverts The number of vertices in the polygon. -/// @param[in] hmin The height of the base of the polygon. -/// @param[in] hmax The height of the top of the polygon. -/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] -/// @param[in,out] chf A populated compact heightfield. -void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts, - const float hmin, const float hmax, unsigned char areaId, - rcCompactHeightfield& chf); - -/// Helper function to offset voncex polygons for rcMarkConvexPolyArea. +/// +/// @param[in,out] context The build context to use during the operation. +/// @param[in] verts The vertices of the polygon [For: (x, y, z) * @p numVerts] +/// @param[in] numVerts The number of vertices in the polygon. +/// @param[in] minY The height of the base of the polygon. [Units: wu] +/// @param[in] maxY The height of the top of the polygon. [Units: wu] +/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] +/// @param[in,out] compactHeightfield A populated compact heightfield. +void rcMarkConvexPolyArea(rcContext* context, const float* verts, int numVerts, + float minY, float maxY, unsigned char areaId, + rcCompactHeightfield& compactHeightfield); + +/// Expands a convex polygon along its vertex normals by the given offset amount. +/// Inserts extra vertices to bevel sharp corners. +/// +/// Helper function to offset convex polygons for rcMarkConvexPolyArea. +/// /// @ingroup recast -/// @param[in] verts The vertices of the polygon [Form: (x, y, z) * @p nverts] -/// @param[in] nverts The number of vertices in the polygon. +/// +/// @param[in] verts The vertices of the polygon [Form: (x, y, z) * @p numVerts] +/// @param[in] numVerts The number of vertices in the polygon. /// @param[in] offset How much to offset the polygon by. [Units: wu] -/// @param[out] outVerts The offset vertices (should hold up to 2 * @p nverts) [Form: (x, y, z) * return value] +/// @param[out] outVerts The offset vertices (should hold up to 2 * @p numVerts) [Form: (x, y, z) * return value] /// @param[in] maxOutVerts The max number of vertices that can be stored to @p outVerts. /// @returns Number of vertices in the offset polygon or 0 if too few vertices in @p outVerts. -int rcOffsetPoly(const float* verts, const int nverts, const float offset, - float* outVerts, const int maxOutVerts); +int rcOffsetPoly(const float* verts, int numVerts, float offset, float* outVerts, int maxOutVerts); -/// Applies the area id to all spans within the specified cylinder. +/// Applies the area id to all spans within the specified y-axis-aligned cylinder. +/// +/// @see rcCompactHeightfield, rcMedianFilterWalkableArea +/// /// @ingroup recast -/// @param[in,out] ctx The build context to use during the operation. -/// @param[in] pos The center of the base of the cylinder. [Form: (x, y, z)] -/// @param[in] r The radius of the cylinder. -/// @param[in] h The height of the cylinder. -/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] -/// @param[in,out] chf A populated compact heightfield. -void rcMarkCylinderArea(rcContext* ctx, const float* pos, - const float r, const float h, unsigned char areaId, - rcCompactHeightfield& chf); +/// +/// @param[in,out] context The build context to use during the operation. +/// @param[in] position The center of the base of the cylinder. [Form: (x, y, z)] [Units: wu] +/// @param[in] radius The radius of the cylinder. [Units: wu] [Limit: > 0] +/// @param[in] height The height of the cylinder. [Units: wu] [Limit: > 0] +/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] +/// @param[in,out] compactHeightfield A populated compact heightfield. +void rcMarkCylinderArea(rcContext* context, const float* position, float radius, float height, + unsigned char areaId, rcCompactHeightfield& compactHeightfield); /// Builds the distance field for the specified compact heightfield. /// @ingroup recast diff --git a/thirdparty/recastnavigation/Recast/Include/RecastAlloc.h b/thirdparty/recastnavigation/Recast/Include/RecastAlloc.h index 1741de9f0e..dcb2f51c30 100644 --- a/thirdparty/recastnavigation/Recast/Include/RecastAlloc.h +++ b/thirdparty/recastnavigation/Recast/Include/RecastAlloc.h @@ -77,7 +77,7 @@ struct rcNewTag {}; inline void* operator new(size_t, const rcNewTag&, void* p) { return p; } inline void operator delete(void*, const rcNewTag&, void*) {} -/// Signed to avoid warnnings when comparing to int loop indexes, and common error with comparing to zero. +/// Signed to avoid warnings when comparing to int loop indexes, and common error with comparing to zero. /// MSVC2010 has a bug where ssize_t is unsigned (!!!). typedef intptr_t rcSizeType; #define RC_SIZE_MAX INTPTR_MAX diff --git a/thirdparty/recastnavigation/Recast/Include/RecastAssert.h b/thirdparty/recastnavigation/Recast/Include/RecastAssert.h index 81705bbe0b..923b0ff56a 100644 --- a/thirdparty/recastnavigation/Recast/Include/RecastAssert.h +++ b/thirdparty/recastnavigation/Recast/Include/RecastAssert.h @@ -19,7 +19,7 @@ #ifndef RECASTASSERT_H #define RECASTASSERT_H -#ifdef NDEBUG +#ifdef RC_DISABLE_ASSERTS // From https://web.archive.org/web/20210117002833/http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/ # define rcAssert(x) do { (void)sizeof(x); } while ((void)(__LINE__==-1), false) diff --git a/thirdparty/recastnavigation/Recast/Source/RecastArea.cpp b/thirdparty/recastnavigation/Recast/Source/RecastArea.cpp index 07fb02189c..7a7091cc88 100644 --- a/thirdparty/recastnavigation/Recast/Source/RecastArea.cpp +++ b/thirdparty/recastnavigation/Recast/Source/RecastArea.cpp @@ -16,573 +16,700 @@ // 3. This notice may not be removed or altered from any source distribution. // -#include -#include -#include -#include -#include #include "Recast.h" #include "RecastAlloc.h" #include "RecastAssert.h" -/// @par -/// -/// Basically, any spans that are closer to a boundary or obstruction than the specified radius -/// are marked as unwalkable. +#include // for memcpy and memset + +/// Sorts the given data in-place using insertion sort. /// -/// This method is usually called immediately after the heightfield has been built. +/// @param data The data to sort +/// @param dataLength The number of elements in @p data +static void insertSort(unsigned char* data, const int dataLength) +{ + for (int valueIndex = 1; valueIndex < dataLength; valueIndex++) + { + const unsigned char value = data[valueIndex]; + int insertionIndex; + for (insertionIndex = valueIndex - 1; insertionIndex >= 0 && data[insertionIndex] > value; insertionIndex--) + { + // Shift over values + data[insertionIndex + 1] = data[insertionIndex]; + } + + // Insert the value in sorted order. + data[insertionIndex + 1] = value; + } +} + +// TODO (graham): This is duplicated in the ConvexVolumeTool in RecastDemo +/// Checks if a point is contained within a polygon /// -/// @see rcCompactHeightfield, rcBuildCompactHeightfield, rcConfig::walkableRadius -bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf) +/// @param[in] numVerts Number of vertices in the polygon +/// @param[in] verts The polygon vertices +/// @param[in] point The point to check +/// @returns true if the point lies within the polygon, false otherwise. +static bool pointInPoly(int numVerts, const float* verts, const float* point) { - rcAssert(ctx); - - const int w = chf.width; - const int h = chf.height; - - rcScopedTimer timer(ctx, RC_TIMER_ERODE_AREA); - - unsigned char* dist = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP); - if (!dist) + bool inPoly = false; + for (int i = 0, j = numVerts - 1; i < numVerts; j = i++) + { + const float* vi = &verts[i * 3]; + const float* vj = &verts[j * 3]; + + if ((vi[2] > point[2]) == (vj[2] > point[2])) + { + continue; + } + + if (point[0] >= (vj[0] - vi[0]) * (point[2] - vi[2]) / (vj[2] - vi[2]) + vi[0]) + { + continue; + } + inPoly = !inPoly; + } + return inPoly; +} + +bool rcErodeWalkableArea(rcContext* context, const int erosionRadius, rcCompactHeightfield& compactHeightfield) +{ + rcAssert(context != NULL); + + const int xSize = compactHeightfield.width; + const int zSize = compactHeightfield.height; + const int& zStride = xSize; // For readability + + rcScopedTimer timer(context, RC_TIMER_ERODE_AREA); + + unsigned char* distanceToBoundary = (unsigned char*)rcAlloc(sizeof(unsigned char) * compactHeightfield.spanCount, + RC_ALLOC_TEMP); + if (!distanceToBoundary) { - ctx->log(RC_LOG_ERROR, "erodeWalkableArea: Out of memory 'dist' (%d).", chf.spanCount); + context->log(RC_LOG_ERROR, "erodeWalkableArea: Out of memory 'dist' (%d).", compactHeightfield.spanCount); return false; } - - // Init distance. - memset(dist, 0xff, sizeof(unsigned char)*chf.spanCount); + memset(distanceToBoundary, 0xff, sizeof(unsigned char) * compactHeightfield.spanCount); // Mark boundary cells. - for (int y = 0; y < h; ++y) + for (int z = 0; z < zSize; ++z) { - for (int x = 0; x < w; ++x) + for (int x = 0; x < xSize; ++x) { - const rcCompactCell& c = chf.cells[x+y*w]; - for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + const rcCompactCell& cell = compactHeightfield.cells[x + z * zStride]; + for (int spanIndex = (int)cell.index, maxSpanIndex = (int)(cell.index + cell.count); spanIndex < maxSpanIndex; ++spanIndex) { - if (chf.areas[i] == RC_NULL_AREA) + if (compactHeightfield.areas[spanIndex] == RC_NULL_AREA) { - dist[i] = 0; + distanceToBoundary[spanIndex] = 0; + continue; } - else + const rcCompactSpan& span = compactHeightfield.spans[spanIndex]; + + // Check that there is a non-null adjacent span in each of the 4 cardinal directions. + int neighborCount = 0; + for (int direction = 0; direction < 4; ++direction) { - const rcCompactSpan& s = chf.spans[i]; - int nc = 0; - for (int dir = 0; dir < 4; ++dir) + const int neighborConnection = rcGetCon(span, direction); + if (neighborConnection == RC_NOT_CONNECTED) { - if (rcGetCon(s, dir) != RC_NOT_CONNECTED) - { - const int nx = x + rcGetDirOffsetX(dir); - const int ny = y + rcGetDirOffsetY(dir); - const int nidx = (int)chf.cells[nx+ny*w].index + rcGetCon(s, dir); - if (chf.areas[nidx] != RC_NULL_AREA) - { - nc++; - } - } + break; } - // At least one missing neighbour. - if (nc != 4) - dist[i] = 0; + + const int neighborX = x + rcGetDirOffsetX(direction); + const int neighborZ = z + rcGetDirOffsetY(direction); + const int neighborSpanIndex = (int)compactHeightfield.cells[neighborX + neighborZ * zStride].index + neighborConnection; + + if (compactHeightfield.areas[neighborSpanIndex] == RC_NULL_AREA) + { + break; + } + neighborCount++; + } + + // At least one missing neighbour, so this is a boundary cell. + if (neighborCount != 4) + { + distanceToBoundary[spanIndex] = 0; } } } } - unsigned char nd; + unsigned char newDistance; // Pass 1 - for (int y = 0; y < h; ++y) + for (int z = 0; z < zSize; ++z) { - for (int x = 0; x < w; ++x) + for (int x = 0; x < xSize; ++x) { - const rcCompactCell& c = chf.cells[x+y*w]; - for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + const rcCompactCell& cell = compactHeightfield.cells[x + z * zStride]; + const int maxSpanIndex = (int)(cell.index + cell.count); + for (int spanIndex = (int)cell.index; spanIndex < maxSpanIndex; ++spanIndex) { - const rcCompactSpan& s = chf.spans[i]; - - if (rcGetCon(s, 0) != RC_NOT_CONNECTED) + const rcCompactSpan& span = compactHeightfield.spans[spanIndex]; + + if (rcGetCon(span, 0) != RC_NOT_CONNECTED) { // (-1,0) - const int ax = x + rcGetDirOffsetX(0); - const int ay = y + rcGetDirOffsetY(0); - const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0); - const rcCompactSpan& as = chf.spans[ai]; - nd = (unsigned char)rcMin((int)dist[ai]+2, 255); - if (nd < dist[i]) - dist[i] = nd; - + const int aX = x + rcGetDirOffsetX(0); + const int aY = z + rcGetDirOffsetY(0); + const int aIndex = (int)compactHeightfield.cells[aX + aY * xSize].index + rcGetCon(span, 0); + const rcCompactSpan& aSpan = compactHeightfield.spans[aIndex]; + newDistance = (unsigned char)rcMin((int)distanceToBoundary[aIndex] + 2, 255); + if (newDistance < distanceToBoundary[spanIndex]) + { + distanceToBoundary[spanIndex] = newDistance; + } + // (-1,-1) - if (rcGetCon(as, 3) != RC_NOT_CONNECTED) + if (rcGetCon(aSpan, 3) != RC_NOT_CONNECTED) { - const int aax = ax + rcGetDirOffsetX(3); - const int aay = ay + rcGetDirOffsetY(3); - const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3); - nd = (unsigned char)rcMin((int)dist[aai]+3, 255); - if (nd < dist[i]) - dist[i] = nd; + const int bX = aX + rcGetDirOffsetX(3); + const int bY = aY + rcGetDirOffsetY(3); + const int bIndex = (int)compactHeightfield.cells[bX + bY * xSize].index + rcGetCon(aSpan, 3); + newDistance = (unsigned char)rcMin((int)distanceToBoundary[bIndex] + 3, 255); + if (newDistance < distanceToBoundary[spanIndex]) + { + distanceToBoundary[spanIndex] = newDistance; + } } } - if (rcGetCon(s, 3) != RC_NOT_CONNECTED) + if (rcGetCon(span, 3) != RC_NOT_CONNECTED) { // (0,-1) - const int ax = x + rcGetDirOffsetX(3); - const int ay = y + rcGetDirOffsetY(3); - const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3); - const rcCompactSpan& as = chf.spans[ai]; - nd = (unsigned char)rcMin((int)dist[ai]+2, 255); - if (nd < dist[i]) - dist[i] = nd; - + const int aX = x + rcGetDirOffsetX(3); + const int aY = z + rcGetDirOffsetY(3); + const int aIndex = (int)compactHeightfield.cells[aX + aY * xSize].index + rcGetCon(span, 3); + const rcCompactSpan& aSpan = compactHeightfield.spans[aIndex]; + newDistance = (unsigned char)rcMin((int)distanceToBoundary[aIndex] + 2, 255); + if (newDistance < distanceToBoundary[spanIndex]) + { + distanceToBoundary[spanIndex] = newDistance; + } + // (1,-1) - if (rcGetCon(as, 2) != RC_NOT_CONNECTED) + if (rcGetCon(aSpan, 2) != RC_NOT_CONNECTED) { - const int aax = ax + rcGetDirOffsetX(2); - const int aay = ay + rcGetDirOffsetY(2); - const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2); - nd = (unsigned char)rcMin((int)dist[aai]+3, 255); - if (nd < dist[i]) - dist[i] = nd; + const int bX = aX + rcGetDirOffsetX(2); + const int bY = aY + rcGetDirOffsetY(2); + const int bIndex = (int)compactHeightfield.cells[bX + bY * xSize].index + rcGetCon(aSpan, 2); + newDistance = (unsigned char)rcMin((int)distanceToBoundary[bIndex] + 3, 255); + if (newDistance < distanceToBoundary[spanIndex]) + { + distanceToBoundary[spanIndex] = newDistance; + } } } } } } - + // Pass 2 - for (int y = h-1; y >= 0; --y) + for (int z = zSize - 1; z >= 0; --z) { - for (int x = w-1; x >= 0; --x) + for (int x = xSize - 1; x >= 0; --x) { - const rcCompactCell& c = chf.cells[x+y*w]; - for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + const rcCompactCell& cell = compactHeightfield.cells[x + z * zStride]; + const int maxSpanIndex = (int)(cell.index + cell.count); + for (int spanIndex = (int)cell.index; spanIndex < maxSpanIndex; ++spanIndex) { - const rcCompactSpan& s = chf.spans[i]; - - if (rcGetCon(s, 2) != RC_NOT_CONNECTED) + const rcCompactSpan& span = compactHeightfield.spans[spanIndex]; + + if (rcGetCon(span, 2) != RC_NOT_CONNECTED) { // (1,0) - const int ax = x + rcGetDirOffsetX(2); - const int ay = y + rcGetDirOffsetY(2); - const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2); - const rcCompactSpan& as = chf.spans[ai]; - nd = (unsigned char)rcMin((int)dist[ai]+2, 255); - if (nd < dist[i]) - dist[i] = nd; - + const int aX = x + rcGetDirOffsetX(2); + const int aY = z + rcGetDirOffsetY(2); + const int aIndex = (int)compactHeightfield.cells[aX + aY * xSize].index + rcGetCon(span, 2); + const rcCompactSpan& aSpan = compactHeightfield.spans[aIndex]; + newDistance = (unsigned char)rcMin((int)distanceToBoundary[aIndex] + 2, 255); + if (newDistance < distanceToBoundary[spanIndex]) + { + distanceToBoundary[spanIndex] = newDistance; + } + // (1,1) - if (rcGetCon(as, 1) != RC_NOT_CONNECTED) + if (rcGetCon(aSpan, 1) != RC_NOT_CONNECTED) { - const int aax = ax + rcGetDirOffsetX(1); - const int aay = ay + rcGetDirOffsetY(1); - const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1); - nd = (unsigned char)rcMin((int)dist[aai]+3, 255); - if (nd < dist[i]) - dist[i] = nd; + const int bX = aX + rcGetDirOffsetX(1); + const int bY = aY + rcGetDirOffsetY(1); + const int bIndex = (int)compactHeightfield.cells[bX + bY * xSize].index + rcGetCon(aSpan, 1); + newDistance = (unsigned char)rcMin((int)distanceToBoundary[bIndex] + 3, 255); + if (newDistance < distanceToBoundary[spanIndex]) + { + distanceToBoundary[spanIndex] = newDistance; + } } } - if (rcGetCon(s, 1) != RC_NOT_CONNECTED) + if (rcGetCon(span, 1) != RC_NOT_CONNECTED) { // (0,1) - const int ax = x + rcGetDirOffsetX(1); - const int ay = y + rcGetDirOffsetY(1); - const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1); - const rcCompactSpan& as = chf.spans[ai]; - nd = (unsigned char)rcMin((int)dist[ai]+2, 255); - if (nd < dist[i]) - dist[i] = nd; - + const int aX = x + rcGetDirOffsetX(1); + const int aY = z + rcGetDirOffsetY(1); + const int aIndex = (int)compactHeightfield.cells[aX + aY * xSize].index + rcGetCon(span, 1); + const rcCompactSpan& aSpan = compactHeightfield.spans[aIndex]; + newDistance = (unsigned char)rcMin((int)distanceToBoundary[aIndex] + 2, 255); + if (newDistance < distanceToBoundary[spanIndex]) + { + distanceToBoundary[spanIndex] = newDistance; + } + // (-1,1) - if (rcGetCon(as, 0) != RC_NOT_CONNECTED) + if (rcGetCon(aSpan, 0) != RC_NOT_CONNECTED) { - const int aax = ax + rcGetDirOffsetX(0); - const int aay = ay + rcGetDirOffsetY(0); - const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0); - nd = (unsigned char)rcMin((int)dist[aai]+3, 255); - if (nd < dist[i]) - dist[i] = nd; + const int bX = aX + rcGetDirOffsetX(0); + const int bY = aY + rcGetDirOffsetY(0); + const int bIndex = (int)compactHeightfield.cells[bX + bY * xSize].index + rcGetCon(aSpan, 0); + newDistance = (unsigned char)rcMin((int)distanceToBoundary[bIndex] + 3, 255); + if (newDistance < distanceToBoundary[spanIndex]) + { + distanceToBoundary[spanIndex] = newDistance; + } } } } } } - - const unsigned char thr = (unsigned char)(radius*2); - for (int i = 0; i < chf.spanCount; ++i) - if (dist[i] < thr) - chf.areas[i] = RC_NULL_AREA; - - rcFree(dist); - - return true; -} -static void insertSort(unsigned char* a, const int n) -{ - int i, j; - for (i = 1; i < n; i++) + const unsigned char minBoundaryDistance = (unsigned char)(erosionRadius * 2); + for (int spanIndex = 0; spanIndex < compactHeightfield.spanCount; ++spanIndex) { - const unsigned char value = a[i]; - for (j = i - 1; j >= 0 && a[j] > value; j--) - a[j+1] = a[j]; - a[j+1] = value; + if (distanceToBoundary[spanIndex] < minBoundaryDistance) + { + compactHeightfield.areas[spanIndex] = RC_NULL_AREA; + } } + + rcFree(distanceToBoundary); + + return true; } -/// @par -/// -/// This filter is usually applied after applying area id's using functions -/// such as #rcMarkBoxArea, #rcMarkConvexPolyArea, and #rcMarkCylinderArea. -/// -/// @see rcCompactHeightfield -bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf) +bool rcMedianFilterWalkableArea(rcContext* context, rcCompactHeightfield& compactHeightfield) { - rcAssert(ctx); - - const int w = chf.width; - const int h = chf.height; + rcAssert(context); - rcScopedTimer timer(ctx, RC_TIMER_MEDIAN_AREA); - - unsigned char* areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP); + const int xSize = compactHeightfield.width; + const int zSize = compactHeightfield.height; + const int zStride = xSize; // For readability + + rcScopedTimer timer(context, RC_TIMER_MEDIAN_AREA); + + unsigned char* areas = (unsigned char*)rcAlloc(sizeof(unsigned char) * compactHeightfield.spanCount, RC_ALLOC_TEMP); if (!areas) { - ctx->log(RC_LOG_ERROR, "medianFilterWalkableArea: Out of memory 'areas' (%d).", chf.spanCount); + context->log(RC_LOG_ERROR, "medianFilterWalkableArea: Out of memory 'areas' (%d).", + compactHeightfield.spanCount); return false; } - - // Init distance. - memset(areas, 0xff, sizeof(unsigned char)*chf.spanCount); - - for (int y = 0; y < h; ++y) + memset(areas, 0xff, sizeof(unsigned char) * compactHeightfield.spanCount); + + for (int z = 0; z < zSize; ++z) { - for (int x = 0; x < w; ++x) + for (int x = 0; x < xSize; ++x) { - const rcCompactCell& c = chf.cells[x+y*w]; - for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + const rcCompactCell& cell = compactHeightfield.cells[x + z * zStride]; + const int maxSpanIndex = (int)(cell.index + cell.count); + for (int spanIndex = (int)cell.index; spanIndex < maxSpanIndex; ++spanIndex) { - const rcCompactSpan& s = chf.spans[i]; - if (chf.areas[i] == RC_NULL_AREA) + const rcCompactSpan& span = compactHeightfield.spans[spanIndex]; + if (compactHeightfield.areas[spanIndex] == RC_NULL_AREA) { - areas[i] = chf.areas[i]; + areas[spanIndex] = compactHeightfield.areas[spanIndex]; continue; } - - unsigned char nei[9]; - for (int j = 0; j < 9; ++j) - nei[j] = chf.areas[i]; - + + unsigned char neighborAreas[9]; + for (int neighborIndex = 0; neighborIndex < 9; ++neighborIndex) + { + neighborAreas[neighborIndex] = compactHeightfield.areas[spanIndex]; + } + for (int dir = 0; dir < 4; ++dir) { - if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + if (rcGetCon(span, dir) == RC_NOT_CONNECTED) + { + continue; + } + + const int aX = x + rcGetDirOffsetX(dir); + const int aZ = z + rcGetDirOffsetY(dir); + const int aIndex = (int)compactHeightfield.cells[aX + aZ * zStride].index + rcGetCon(span, dir); + if (compactHeightfield.areas[aIndex] != RC_NULL_AREA) + { + neighborAreas[dir * 2 + 0] = compactHeightfield.areas[aIndex]; + } + + const rcCompactSpan& aSpan = compactHeightfield.spans[aIndex]; + const int dir2 = (dir + 1) & 0x3; + const int neighborConnection2 = rcGetCon(aSpan, dir2); + if (neighborConnection2 != RC_NOT_CONNECTED) { - const int ax = x + rcGetDirOffsetX(dir); - const int ay = y + rcGetDirOffsetY(dir); - const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); - if (chf.areas[ai] != RC_NULL_AREA) - nei[dir*2+0] = chf.areas[ai]; - - const rcCompactSpan& as = chf.spans[ai]; - const int dir2 = (dir+1) & 0x3; - if (rcGetCon(as, dir2) != RC_NOT_CONNECTED) + const int bX = aX + rcGetDirOffsetX(dir2); + const int bZ = aZ + rcGetDirOffsetY(dir2); + const int bIndex = (int)compactHeightfield.cells[bX + bZ * zStride].index + neighborConnection2; + if (compactHeightfield.areas[bIndex] != RC_NULL_AREA) { - const int ax2 = ax + rcGetDirOffsetX(dir2); - const int ay2 = ay + rcGetDirOffsetY(dir2); - const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2); - if (chf.areas[ai2] != RC_NULL_AREA) - nei[dir*2+1] = chf.areas[ai2]; + neighborAreas[dir * 2 + 1] = compactHeightfield.areas[bIndex]; } } } - insertSort(nei, 9); - areas[i] = nei[4]; + insertSort(neighborAreas, 9); + areas[spanIndex] = neighborAreas[4]; } } } - - memcpy(chf.areas, areas, sizeof(unsigned char)*chf.spanCount); - + + memcpy(compactHeightfield.areas, areas, sizeof(unsigned char) * compactHeightfield.spanCount); + rcFree(areas); - + return true; } -/// @par -/// -/// The value of spacial parameters are in world units. -/// -/// @see rcCompactHeightfield, rcMedianFilterWalkableArea -void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId, - rcCompactHeightfield& chf) +void rcMarkBoxArea(rcContext* context, const float* boxMinBounds, const float* boxMaxBounds, unsigned char areaId, + rcCompactHeightfield& compactHeightfield) { - rcAssert(ctx); - - rcScopedTimer timer(ctx, RC_TIMER_MARK_BOX_AREA); - - int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs); - int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch); - int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs); - int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs); - int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch); - int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs); - - if (maxx < 0) return; - if (minx >= chf.width) return; - if (maxz < 0) return; - if (minz >= chf.height) return; - - if (minx < 0) minx = 0; - if (maxx >= chf.width) maxx = chf.width-1; - if (minz < 0) minz = 0; - if (maxz >= chf.height) maxz = chf.height-1; - - for (int z = minz; z <= maxz; ++z) + rcAssert(context); + + rcScopedTimer timer(context, RC_TIMER_MARK_BOX_AREA); + + const int xSize = compactHeightfield.width; + const int zSize = compactHeightfield.height; + const int zStride = xSize; // For readability + + // Find the footprint of the box area in grid cell coordinates. + int minX = (int)((boxMinBounds[0] - compactHeightfield.bmin[0]) / compactHeightfield.cs); + int minY = (int)((boxMinBounds[1] - compactHeightfield.bmin[1]) / compactHeightfield.ch); + int minZ = (int)((boxMinBounds[2] - compactHeightfield.bmin[2]) / compactHeightfield.cs); + int maxX = (int)((boxMaxBounds[0] - compactHeightfield.bmin[0]) / compactHeightfield.cs); + int maxY = (int)((boxMaxBounds[1] - compactHeightfield.bmin[1]) / compactHeightfield.ch); + int maxZ = (int)((boxMaxBounds[2] - compactHeightfield.bmin[2]) / compactHeightfield.cs); + + // Early-out if the box is outside the bounds of the grid. + if (maxX < 0) { return; } + if (minX >= xSize) { return; } + if (maxZ < 0) { return; } + if (minZ >= zSize) { return; } + + // Clamp relevant bound coordinates to the grid. + if (minX < 0) { minX = 0; } + if (maxX >= xSize) { maxX = xSize - 1; } + if (minZ < 0) { minZ = 0; } + if (maxZ >= zSize) { maxZ = zSize - 1; } + + // Mark relevant cells. + for (int z = minZ; z <= maxZ; ++z) { - for (int x = minx; x <= maxx; ++x) + for (int x = minX; x <= maxX; ++x) { - const rcCompactCell& c = chf.cells[x+z*chf.width]; - for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + const rcCompactCell& cell = compactHeightfield.cells[x + z * zStride]; + const int maxSpanIndex = (int)(cell.index + cell.count); + for (int spanIndex = (int)cell.index; spanIndex < maxSpanIndex; ++spanIndex) { - rcCompactSpan& s = chf.spans[i]; - if ((int)s.y >= miny && (int)s.y <= maxy) + rcCompactSpan& span = compactHeightfield.spans[spanIndex]; + + // Skip if the span is outside the box extents. + if ((int)span.y < minY || (int)span.y > maxY) { - if (chf.areas[i] != RC_NULL_AREA) - chf.areas[i] = areaId; + continue; } + + // Skip if the span has been removed. + if (compactHeightfield.areas[spanIndex] == RC_NULL_AREA) + { + continue; + } + + // Mark the span. + compactHeightfield.areas[spanIndex] = areaId; } } } } - -static int pointInPoly(int nvert, const float* verts, const float* p) +void rcMarkConvexPolyArea(rcContext* context, const float* verts, const int numVerts, + const float minY, const float maxY, unsigned char areaId, + rcCompactHeightfield& compactHeightfield) { - int i, j, c = 0; - for (i = 0, j = nvert-1; i < nvert; j = i++) - { - const float* vi = &verts[i*3]; - const float* vj = &verts[j*3]; - if (((vi[2] > p[2]) != (vj[2] > p[2])) && - (p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) - c = !c; - } - return c; -} + rcAssert(context); -/// @par -/// -/// The value of spacial parameters are in world units. -/// -/// The y-values of the polygon vertices are ignored. So the polygon is effectively -/// projected onto the xz-plane at @p hmin, then extruded to @p hmax. -/// -/// @see rcCompactHeightfield, rcMedianFilterWalkableArea -void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts, - const float hmin, const float hmax, unsigned char areaId, - rcCompactHeightfield& chf) -{ - rcAssert(ctx); - - rcScopedTimer timer(ctx, RC_TIMER_MARK_CONVEXPOLY_AREA); + rcScopedTimer timer(context, RC_TIMER_MARK_CONVEXPOLY_AREA); - float bmin[3], bmax[3]; + const int xSize = compactHeightfield.width; + const int zSize = compactHeightfield.height; + const int zStride = xSize; // For readability + + // Compute the bounding box of the polygon + float bmin[3]; + float bmax[3]; rcVcopy(bmin, verts); rcVcopy(bmax, verts); - for (int i = 1; i < nverts; ++i) + for (int i = 1; i < numVerts; ++i) { - rcVmin(bmin, &verts[i*3]); - rcVmax(bmax, &verts[i*3]); + rcVmin(bmin, &verts[i * 3]); + rcVmax(bmax, &verts[i * 3]); } - bmin[1] = hmin; - bmax[1] = hmax; - - int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs); - int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch); - int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs); - int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs); - int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch); - int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs); - - if (maxx < 0) return; - if (minx >= chf.width) return; - if (maxz < 0) return; - if (minz >= chf.height) return; - - if (minx < 0) minx = 0; - if (maxx >= chf.width) maxx = chf.width-1; - if (minz < 0) minz = 0; - if (maxz >= chf.height) maxz = chf.height-1; - - + bmin[1] = minY; + bmax[1] = maxY; + + // Compute the grid footprint of the polygon + int minx = (int)((bmin[0] - compactHeightfield.bmin[0]) / compactHeightfield.cs); + int miny = (int)((bmin[1] - compactHeightfield.bmin[1]) / compactHeightfield.ch); + int minz = (int)((bmin[2] - compactHeightfield.bmin[2]) / compactHeightfield.cs); + int maxx = (int)((bmax[0] - compactHeightfield.bmin[0]) / compactHeightfield.cs); + int maxy = (int)((bmax[1] - compactHeightfield.bmin[1]) / compactHeightfield.ch); + int maxz = (int)((bmax[2] - compactHeightfield.bmin[2]) / compactHeightfield.cs); + + // Early-out if the polygon lies entirely outside the grid. + if (maxx < 0) { return; } + if (minx >= xSize) { return; } + if (maxz < 0) { return; } + if (minz >= zSize) { return; } + + // Clamp the polygon footprint to the grid + if (minx < 0) { minx = 0; } + if (maxx >= xSize) { maxx = xSize - 1; } + if (minz < 0) { minz = 0; } + if (maxz >= zSize) { maxz = zSize - 1; } + // TODO: Optimize. for (int z = minz; z <= maxz; ++z) { for (int x = minx; x <= maxx; ++x) { - const rcCompactCell& c = chf.cells[x+z*chf.width]; - for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + const rcCompactCell& cell = compactHeightfield.cells[x + z * zStride]; + const int maxSpanIndex = (int)(cell.index + cell.count); + for (int spanIndex = (int)cell.index; spanIndex < maxSpanIndex; ++spanIndex) { - rcCompactSpan& s = chf.spans[i]; - if (chf.areas[i] == RC_NULL_AREA) + rcCompactSpan& span = compactHeightfield.spans[spanIndex]; + + // Skip if span is removed. + if (compactHeightfield.areas[spanIndex] == RC_NULL_AREA) + { continue; - if ((int)s.y >= miny && (int)s.y <= maxy) + } + + // Skip if y extents don't overlap. + if ((int)span.y < miny || (int)span.y > maxy) { - float p[3]; - p[0] = chf.bmin[0] + (x+0.5f)*chf.cs; - p[1] = 0; - p[2] = chf.bmin[2] + (z+0.5f)*chf.cs; + continue; + } - if (pointInPoly(nverts, verts, p)) - { - chf.areas[i] = areaId; - } + const float point[] = { + compactHeightfield.bmin[0] + ((float)x + 0.5f) * compactHeightfield.cs, + 0, + compactHeightfield.bmin[2] + ((float)z + 0.5f) * compactHeightfield.cs + }; + + if (pointInPoly(numVerts, verts, point)) + { + compactHeightfield.areas[spanIndex] = areaId; } } } } } -int rcOffsetPoly(const float* verts, const int nverts, const float offset, - float* outVerts, const int maxOutVerts) +static const float EPSILON = 1e-6f; + +/// Normalizes the vector if the length is greater than zero. +/// If the magnitude is zero, the vector is unchanged. +/// @param[in,out] v The vector to normalize. [(x, y, z)] +static void rcVsafeNormalize(float* v) { - const float MITER_LIMIT = 1.20f; + const float sqMag = rcSqr(v[0]) + rcSqr(v[1]) + rcSqr(v[2]); + if (sqMag > EPSILON) + { + const float inverseMag = 1.0f / rcSqrt(sqMag); + v[0] *= inverseMag; + v[1] *= inverseMag; + v[2] *= inverseMag; + } +} - int n = 0; +int rcOffsetPoly(const float* verts, const int numVerts, const float offset, float* outVerts, const int maxOutVerts) +{ + // Defines the limit at which a miter becomes a bevel. + // Similar in behavior to https://developer.mozilla.org/en-US/docs/Web/SVG/Attribute/stroke-miterlimit + const float MITER_LIMIT = 1.20f; + + int numOutVerts = 0; - for (int i = 0; i < nverts; i++) + for (int vertIndex = 0; vertIndex < numVerts; vertIndex++) { - const int a = (i+nverts-1) % nverts; - const int b = i; - const int c = (i+1) % nverts; - const float* va = &verts[a*3]; - const float* vb = &verts[b*3]; - const float* vc = &verts[c*3]; - float dx0 = vb[0] - va[0]; - float dy0 = vb[2] - va[2]; - float d0 = dx0*dx0 + dy0*dy0; - if (d0 > 1e-6f) - { - d0 = 1.0f/rcSqrt(d0); - dx0 *= d0; - dy0 *= d0; - } - float dx1 = vc[0] - vb[0]; - float dy1 = vc[2] - vb[2]; - float d1 = dx1*dx1 + dy1*dy1; - if (d1 > 1e-6f) - { - d1 = 1.0f/rcSqrt(d1); - dx1 *= d1; - dy1 *= d1; - } - const float dlx0 = -dy0; - const float dly0 = dx0; - const float dlx1 = -dy1; - const float dly1 = dx1; - float cross = dx1*dy0 - dx0*dy1; - float dmx = (dlx0 + dlx1) * 0.5f; - float dmy = (dly0 + dly1) * 0.5f; - float dmr2 = dmx*dmx + dmy*dmy; - bool bevel = dmr2 * MITER_LIMIT*MITER_LIMIT < 1.0f; - if (dmr2 > 1e-6f) + // Grab three vertices of the polygon. + const int vertIndexA = (vertIndex + numVerts - 1) % numVerts; + const int vertIndexB = vertIndex; + const int vertIndexC = (vertIndex + 1) % numVerts; + const float* vertA = &verts[vertIndexA * 3]; + const float* vertB = &verts[vertIndexB * 3]; + const float* vertC = &verts[vertIndexC * 3]; + + // From A to B on the x/z plane + float prevSegmentDir[3]; + rcVsub(prevSegmentDir, vertB, vertA); + prevSegmentDir[1] = 0; // Squash onto x/z plane + rcVsafeNormalize(prevSegmentDir); + + // From B to C on the x/z plane + float currSegmentDir[3]; + rcVsub(currSegmentDir, vertC, vertB); + currSegmentDir[1] = 0; // Squash onto x/z plane + rcVsafeNormalize(currSegmentDir); + + // The y component of the cross product of the two normalized segment directions. + // The X and Z components of the cross product are both zero because the two + // segment direction vectors fall within the x/z plane. + float cross = currSegmentDir[0] * prevSegmentDir[2] - prevSegmentDir[0] * currSegmentDir[2]; + + // CCW perpendicular vector to AB. The segment normal. + const float prevSegmentNormX = -prevSegmentDir[2]; + const float prevSegmentNormZ = prevSegmentDir[0]; + + // CCW perpendicular vector to BC. The segment normal. + const float currSegmentNormX = -currSegmentDir[2]; + const float currSegmentNormZ = currSegmentDir[0]; + + // Average the two segment normals to get the proportional miter offset for B. + // This isn't normalized because it's defining the distance and direction the corner will need to be + // adjusted proportionally to the edge offsets to properly miter the adjoining edges. + float cornerMiterX = (prevSegmentNormX + currSegmentNormX) * 0.5f; + float cornerMiterZ = (prevSegmentNormZ + currSegmentNormZ) * 0.5f; + const float cornerMiterSqMag = rcSqr(cornerMiterX) + rcSqr(cornerMiterZ); + + // If the magnitude of the segment normal average is less than about .69444, + // the corner is an acute enough angle that the result should be beveled. + const bool bevel = cornerMiterSqMag * MITER_LIMIT * MITER_LIMIT < 1.0f; + + // Scale the corner miter so it's proportional to how much the corner should be offset compared to the edges. + if (cornerMiterSqMag > EPSILON) { - const float scale = 1.0f / dmr2; - dmx *= scale; - dmy *= scale; + const float scale = 1.0f / cornerMiterSqMag; + cornerMiterX *= scale; + cornerMiterZ *= scale; } - if (bevel && cross < 0.0f) + if (bevel && cross < 0.0f) // If the corner is convex and an acute enough angle, generate a bevel. { - if (n+2 >= maxOutVerts) + if (numOutVerts + 2 > maxOutVerts) + { return 0; - float d = (1.0f - (dx0*dx1 + dy0*dy1))*0.5f; - outVerts[n*3+0] = vb[0] + (-dlx0+dx0*d)*offset; - outVerts[n*3+1] = vb[1]; - outVerts[n*3+2] = vb[2] + (-dly0+dy0*d)*offset; - n++; - outVerts[n*3+0] = vb[0] + (-dlx1-dx1*d)*offset; - outVerts[n*3+1] = vb[1]; - outVerts[n*3+2] = vb[2] + (-dly1-dy1*d)*offset; - n++; + } + + // Generate two bevel vertices at a distances from B proportional to the angle between the two segments. + // Move each bevel vertex out proportional to the given offset. + float d = (1.0f - (prevSegmentDir[0] * currSegmentDir[0] + prevSegmentDir[2] * currSegmentDir[2])) * 0.5f; + + outVerts[numOutVerts * 3 + 0] = vertB[0] + (-prevSegmentNormX + prevSegmentDir[0] * d) * offset; + outVerts[numOutVerts * 3 + 1] = vertB[1]; + outVerts[numOutVerts * 3 + 2] = vertB[2] + (-prevSegmentNormZ + prevSegmentDir[2] * d) * offset; + numOutVerts++; + + outVerts[numOutVerts * 3 + 0] = vertB[0] + (-currSegmentNormX - currSegmentDir[0] * d) * offset; + outVerts[numOutVerts * 3 + 1] = vertB[1]; + outVerts[numOutVerts * 3 + 2] = vertB[2] + (-currSegmentNormZ - currSegmentDir[2] * d) * offset; + numOutVerts++; } else { - if (n+1 >= maxOutVerts) + if (numOutVerts + 1 > maxOutVerts) + { return 0; - outVerts[n*3+0] = vb[0] - dmx*offset; - outVerts[n*3+1] = vb[1]; - outVerts[n*3+2] = vb[2] - dmy*offset; - n++; + } + + // Move B along the miter direction by the specified offset. + outVerts[numOutVerts * 3 + 0] = vertB[0] - cornerMiterX * offset; + outVerts[numOutVerts * 3 + 1] = vertB[1]; + outVerts[numOutVerts * 3 + 2] = vertB[2] - cornerMiterZ * offset; + numOutVerts++; } } - - return n; -} + return numOutVerts; +} -/// @par -/// -/// The value of spacial parameters are in world units. -/// -/// @see rcCompactHeightfield, rcMedianFilterWalkableArea -void rcMarkCylinderArea(rcContext* ctx, const float* pos, - const float r, const float h, unsigned char areaId, - rcCompactHeightfield& chf) +void rcMarkCylinderArea(rcContext* context, const float* position, const float radius, const float height, + unsigned char areaId, rcCompactHeightfield& compactHeightfield) { - rcAssert(ctx); - - rcScopedTimer timer(ctx, RC_TIMER_MARK_CYLINDER_AREA); - - float bmin[3], bmax[3]; - bmin[0] = pos[0] - r; - bmin[1] = pos[1]; - bmin[2] = pos[2] - r; - bmax[0] = pos[0] + r; - bmax[1] = pos[1] + h; - bmax[2] = pos[2] + r; - const float r2 = r*r; - - int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs); - int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch); - int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs); - int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs); - int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch); - int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs); - - if (maxx < 0) return; - if (minx >= chf.width) return; - if (maxz < 0) return; - if (minz >= chf.height) return; - - if (minx < 0) minx = 0; - if (maxx >= chf.width) maxx = chf.width-1; - if (minz < 0) minz = 0; - if (maxz >= chf.height) maxz = chf.height-1; - - + rcAssert(context); + + rcScopedTimer timer(context, RC_TIMER_MARK_CYLINDER_AREA); + + const int xSize = compactHeightfield.width; + const int zSize = compactHeightfield.height; + const int zStride = xSize; // For readability + + // Compute the bounding box of the cylinder + const float cylinderBBMin[] = + { + position[0] - radius, + position[1], + position[2] - radius + }; + const float cylinderBBMax[] = + { + position[0] + radius, + position[1] + height, + position[2] + radius + }; + + // Compute the grid footprint of the cylinder + int minx = (int)((cylinderBBMin[0] - compactHeightfield.bmin[0]) / compactHeightfield.cs); + int miny = (int)((cylinderBBMin[1] - compactHeightfield.bmin[1]) / compactHeightfield.ch); + int minz = (int)((cylinderBBMin[2] - compactHeightfield.bmin[2]) / compactHeightfield.cs); + int maxx = (int)((cylinderBBMax[0] - compactHeightfield.bmin[0]) / compactHeightfield.cs); + int maxy = (int)((cylinderBBMax[1] - compactHeightfield.bmin[1]) / compactHeightfield.ch); + int maxz = (int)((cylinderBBMax[2] - compactHeightfield.bmin[2]) / compactHeightfield.cs); + + // Early-out if the cylinder is completely outside the grid bounds. + if (maxx < 0) { return; } + if (minx >= xSize) { return; } + if (maxz < 0) { return; } + if (minz >= zSize) { return; } + + // Clamp the cylinder bounds to the grid. + if (minx < 0) { minx = 0; } + if (maxx >= xSize) { maxx = xSize - 1; } + if (minz < 0) { minz = 0; } + if (maxz >= zSize) { maxz = zSize - 1; } + + const float radiusSq = radius * radius; + for (int z = minz; z <= maxz; ++z) { for (int x = minx; x <= maxx; ++x) { - const rcCompactCell& c = chf.cells[x+z*chf.width]; - for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + const rcCompactCell& cell = compactHeightfield.cells[x + z * zStride]; + const int maxSpanIndex = (int)(cell.index + cell.count); + + const float cellX = compactHeightfield.bmin[0] + ((float)x + 0.5f) * compactHeightfield.cs; + const float cellZ = compactHeightfield.bmin[2] + ((float)z + 0.5f) * compactHeightfield.cs; + const float deltaX = cellX - position[0]; + const float deltaZ = cellZ - position[2]; + + // Skip this column if it's too far from the center point of the cylinder. + if (rcSqr(deltaX) + rcSqr(deltaZ) >= radiusSq) + { + continue; + } + + // Mark all overlapping spans + for (int spanIndex = (int)cell.index; spanIndex < maxSpanIndex; ++spanIndex) { - rcCompactSpan& s = chf.spans[i]; - - if (chf.areas[i] == RC_NULL_AREA) + rcCompactSpan& span = compactHeightfield.spans[spanIndex]; + + // Skip if span is removed. + if (compactHeightfield.areas[spanIndex] == RC_NULL_AREA) + { continue; - - if ((int)s.y >= miny && (int)s.y <= maxy) + } + + // Mark if y extents overlap. + if ((int)span.y >= miny && (int)span.y <= maxy) { - const float sx = chf.bmin[0] + (x+0.5f)*chf.cs; - const float sz = chf.bmin[2] + (z+0.5f)*chf.cs; - const float dx = sx - pos[0]; - const float dz = sz - pos[2]; - - if (dx*dx + dz*dz < r2) - { - chf.areas[i] = areaId; - } + compactHeightfield.areas[spanIndex] = areaId; } } } diff --git a/thirdparty/recastnavigation/Recast/Source/RecastAssert.cpp b/thirdparty/recastnavigation/Recast/Source/RecastAssert.cpp index 973b681121..2d899ca407 100644 --- a/thirdparty/recastnavigation/Recast/Source/RecastAssert.cpp +++ b/thirdparty/recastnavigation/Recast/Source/RecastAssert.cpp @@ -18,7 +18,7 @@ #include "RecastAssert.h" -#ifndef NDEBUG +#ifndef RC_DISABLE_ASSERTS static rcAssertFailFunc* sRecastAssertFailFunc = 0; diff --git a/thirdparty/recastnavigation/Recast/Source/RecastContour.cpp b/thirdparty/recastnavigation/Recast/Source/RecastContour.cpp index 5508a98a7b..39371f8937 100644 --- a/thirdparty/recastnavigation/Recast/Source/RecastContour.cpp +++ b/thirdparty/recastnavigation/Recast/Source/RecastContour.cpp @@ -399,7 +399,7 @@ static void simplifyContour(rcIntArray& points, rcIntArray& simplified, if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen) { // Round based on the segments in lexilogical order so that the - // max tesselation is consistent regardles in which direction + // max tesselation is consistent regardless in which direction // segments are traversed. const int n = bi < ai ? (bi+pn - ai) : (bi - ai); if (n > 1) @@ -512,7 +512,7 @@ static bool intersectProp(const int* a, const int* b, const int* c, const int* d } // Returns T iff (a,b,c) are collinear and point c lies -// on the closed segement ab. +// on the closed segment ab. static bool between(const int* a, const int* b, const int* c) { if (!collinear(a, b, c)) @@ -749,7 +749,7 @@ static void mergeRegionHoles(rcContext* ctx, rcContourRegion& region) for (int iter = 0; iter < hole->nverts; iter++) { // Find potential diagonals. - // The 'best' vertex must be in the cone described by 3 cosequtive vertices of the outline. + // The 'best' vertex must be in the cone described by 3 consecutive vertices of the outline. // ..o j-1 // | // | * best diff --git a/thirdparty/recastnavigation/Recast/Source/RecastFilter.cpp b/thirdparty/recastnavigation/Recast/Source/RecastFilter.cpp index b5adba43ea..7875040b9f 100644 --- a/thirdparty/recastnavigation/Recast/Source/RecastFilter.cpp +++ b/thirdparty/recastnavigation/Recast/Source/RecastFilter.cpp @@ -21,6 +21,11 @@ #include +namespace +{ + const int MAX_HEIGHTFIELD_HEIGHT = 0xffff; // TODO (graham): Move this to a more visible constant and update usages. +} + void rcFilterLowHangingWalkableObstacles(rcContext* context, const int walkableClimb, rcHeightfield& heightfield) { rcAssert(context); @@ -36,31 +41,30 @@ void rcFilterLowHangingWalkableObstacles(rcContext* context, const int walkableC { rcSpan* previousSpan = NULL; bool previousWasWalkable = false; - unsigned char previousArea = RC_NULL_AREA; + unsigned char previousAreaID = RC_NULL_AREA; + // For each span in the column... for (rcSpan* span = heightfield.spans[x + z * xSize]; span != NULL; previousSpan = span, span = span->next) { const bool walkable = span->area != RC_NULL_AREA; - // If current span is not walkable, but there is walkable - // span just below it, mark the span above it walkable too. - if (!walkable && previousWasWalkable) + + // If current span is not walkable, but there is walkable span just below it and the height difference + // is small enough for the agent to walk over, mark the current span as walkable too. + if (!walkable && previousWasWalkable && (int)span->smax - (int)previousSpan->smax <= walkableClimb) { - if (rcAbs((int)span->smax - (int)previousSpan->smax) <= walkableClimb) - { - span->area = previousArea; - } + span->area = previousAreaID; } - // Copy walkable flag so that it cannot propagate - // past multiple non-walkable objects. + + // Copy the original walkable value regardless of whether we changed it. + // This prevents multiple consecutive non-walkable spans from being erroneously marked as walkable. previousWasWalkable = walkable; - previousArea = span->area; + previousAreaID = span->area; } } } } -void rcFilterLedgeSpans(rcContext* context, const int walkableHeight, const int walkableClimb, - rcHeightfield& heightfield) +void rcFilterLedgeSpans(rcContext* context, const int walkableHeight, const int walkableClimb, rcHeightfield& heightfield) { rcAssert(context); @@ -68,84 +72,99 @@ void rcFilterLedgeSpans(rcContext* context, const int walkableHeight, const int const int xSize = heightfield.width; const int zSize = heightfield.height; - const int MAX_HEIGHT = 0xffff; // TODO (graham): Move this to a more visible constant and update usages. - // Mark border spans. + // Mark spans that are adjacent to a ledge as unwalkable.. for (int z = 0; z < zSize; ++z) { for (int x = 0; x < xSize; ++x) { for (rcSpan* span = heightfield.spans[x + z * xSize]; span; span = span->next) { - // Skip non walkable spans. + // Skip non-walkable spans. if (span->area == RC_NULL_AREA) { continue; } - const int bot = (int)(span->smax); - const int top = span->next ? (int)(span->next->smin) : MAX_HEIGHT; + const int floor = (int)(span->smax); + const int ceiling = span->next ? (int)(span->next->smin) : MAX_HEIGHTFIELD_HEIGHT; - // Find neighbours minimum height. - int minNeighborHeight = MAX_HEIGHT; + // The difference between this walkable area and the lowest neighbor walkable area. + // This is the difference between the current span and all neighbor spans that have + // enough space for an agent to move between, but not accounting at all for surface slope. + int lowestNeighborFloorDifference = MAX_HEIGHTFIELD_HEIGHT; // Min and max height of accessible neighbours. - int accessibleNeighborMinHeight = span->smax; - int accessibleNeighborMaxHeight = span->smax; + int lowestTraversableNeighborFloor = span->smax; + int highestTraversableNeighborFloor = span->smax; for (int direction = 0; direction < 4; ++direction) { - int dx = x + rcGetDirOffsetX(direction); - int dy = z + rcGetDirOffsetY(direction); + const int neighborX = x + rcGetDirOffsetX(direction); + const int neighborZ = z + rcGetDirOffsetY(direction); + // Skip neighbours which are out of bounds. - if (dx < 0 || dy < 0 || dx >= xSize || dy >= zSize) + if (neighborX < 0 || neighborZ < 0 || neighborX >= xSize || neighborZ >= zSize) { - minNeighborHeight = rcMin(minNeighborHeight, -walkableClimb - bot); - continue; + lowestNeighborFloorDifference = -walkableClimb - 1; + break; } - // From minus infinity to the first span. - const rcSpan* neighborSpan = heightfield.spans[dx + dy * xSize]; - int neighborBot = -walkableClimb; - int neighborTop = neighborSpan ? (int)neighborSpan->smin : MAX_HEIGHT; - + const rcSpan* neighborSpan = heightfield.spans[neighborX + neighborZ * xSize]; + + // The most we can step down to the neighbor is the walkableClimb distance. + // Start with the area under the neighbor span + int neighborCeiling = neighborSpan ? (int)neighborSpan->smin : MAX_HEIGHTFIELD_HEIGHT; + // Skip neighbour if the gap between the spans is too small. - if (rcMin(top, neighborTop) - rcMax(bot, neighborBot) > walkableHeight) + if (rcMin(ceiling, neighborCeiling) - floor >= walkableHeight) { - minNeighborHeight = rcMin(minNeighborHeight, neighborBot - bot); + lowestNeighborFloorDifference = (-walkableClimb - 1); + break; } - // Rest of the spans. - for (neighborSpan = heightfield.spans[dx + dy * xSize]; neighborSpan; neighborSpan = neighborSpan->next) + // For each span in the neighboring column... + for (; neighborSpan != NULL; neighborSpan = neighborSpan->next) { - neighborBot = (int)neighborSpan->smax; - neighborTop = neighborSpan->next ? (int)neighborSpan->next->smin : MAX_HEIGHT; - - // Skip neighbour if the gap between the spans is too small. - if (rcMin(top, neighborTop) - rcMax(bot, neighborBot) > walkableHeight) + const int neighborFloor = (int)neighborSpan->smax; + neighborCeiling = neighborSpan->next ? (int)neighborSpan->next->smin : MAX_HEIGHTFIELD_HEIGHT; + + // Only consider neighboring areas that have enough overlap to be potentially traversable. + if (rcMin(ceiling, neighborCeiling) - rcMax(floor, neighborFloor) < walkableHeight) { - minNeighborHeight = rcMin(minNeighborHeight, neighborBot - bot); + // No space to traverse between them. + continue; + } - // Find min/max accessible neighbour height. - if (rcAbs(neighborBot - bot) <= walkableClimb) - { - if (neighborBot < accessibleNeighborMinHeight) accessibleNeighborMinHeight = neighborBot; - if (neighborBot > accessibleNeighborMaxHeight) accessibleNeighborMaxHeight = neighborBot; - } + const int neighborFloorDifference = neighborFloor - floor; + lowestNeighborFloorDifference = rcMin(lowestNeighborFloorDifference, neighborFloorDifference); + // Find min/max accessible neighbor height. + // Only consider neighbors that are at most walkableClimb away. + if (rcAbs(neighborFloorDifference) <= walkableClimb) + { + // There is space to move to the neighbor cell and the slope isn't too much. + lowestTraversableNeighborFloor = rcMin(lowestTraversableNeighborFloor, neighborFloor); + highestTraversableNeighborFloor = rcMax(highestTraversableNeighborFloor, neighborFloor); + } + else if (neighborFloorDifference < -walkableClimb) + { + // We already know this will be considered a ledge span so we can early-out + break; } } } - // The current span is close to a ledge if the drop to any - // neighbour span is less than the walkableClimb. - if (minNeighborHeight < -walkableClimb) + // The current span is close to a ledge if the magnitude of the drop to any neighbour span is greater than the walkableClimb distance. + // That is, there is a gap that is large enough to let an agent move between them, but the drop (surface slope) is too large to allow it. + // (If this is the case, then biggestNeighborStepDown will be negative, so compare against the negative walkableClimb as a means of checking + // the magnitude of the delta) + if (lowestNeighborFloorDifference < -walkableClimb) { span->area = RC_NULL_AREA; } - // If the difference between all neighbours is too large, - // we are at steep slope, mark the span as ledge. - else if ((accessibleNeighborMaxHeight - accessibleNeighborMinHeight) > walkableClimb) + // If the difference between all neighbor floors is too large, this is a steep slope, so mark the span as an unwalkable ledge. + else if (highestTraversableNeighborFloor - lowestTraversableNeighborFloor > walkableClimb) { span->area = RC_NULL_AREA; } @@ -157,13 +176,11 @@ void rcFilterLedgeSpans(rcContext* context, const int walkableHeight, const int void rcFilterWalkableLowHeightSpans(rcContext* context, const int walkableHeight, rcHeightfield& heightfield) { rcAssert(context); - rcScopedTimer timer(context, RC_TIMER_FILTER_WALKABLE); - + const int xSize = heightfield.width; const int zSize = heightfield.height; - const int MAX_HEIGHT = 0xffff; - + // Remove walkable flag from spans which do not have enough // space above them for the agent to stand there. for (int z = 0; z < zSize; ++z) @@ -172,9 +189,9 @@ void rcFilterWalkableLowHeightSpans(rcContext* context, const int walkableHeight { for (rcSpan* span = heightfield.spans[x + z*xSize]; span; span = span->next) { - const int bot = (int)(span->smax); - const int top = span->next ? (int)(span->next->smin) : MAX_HEIGHT; - if ((top - bot) < walkableHeight) + const int floor = (int)(span->smax); + const int ceiling = span->next ? (int)(span->next->smin) : MAX_HEIGHTFIELD_HEIGHT; + if (ceiling - floor < walkableHeight) { span->area = RC_NULL_AREA; } diff --git a/thirdparty/recastnavigation/Recast/Source/RecastMesh.cpp b/thirdparty/recastnavigation/Recast/Source/RecastMesh.cpp index c2c0d51749..c9eb60a5da 100644 --- a/thirdparty/recastnavigation/Recast/Source/RecastMesh.cpp +++ b/thirdparty/recastnavigation/Recast/Source/RecastMesh.cpp @@ -216,8 +216,8 @@ static bool between(const int* a, const int* b, const int* c) // If ab not vertical, check betweenness on x; else on y. if (a[0] != b[0]) return ((a[0] <= c[0]) && (c[0] <= b[0])) || ((a[0] >= c[0]) && (c[0] >= b[0])); - else - return ((a[2] <= c[2]) && (c[2] <= b[2])) || ((a[2] >= c[2]) && (c[2] >= b[2])); + + return ((a[2] <= c[2]) && (c[2] <= b[2])) || ((a[2] >= c[2]) && (c[2] >= b[2])); } // Returns true iff segments ab and cd intersect, properly or improperly. @@ -225,11 +225,12 @@ static bool intersect(const int* a, const int* b, const int* c, const int* d) { if (intersectProp(a, b, c, d)) return true; - else if (between(a, b, c) || between(a, b, d) || - between(c, d, a) || between(c, d, b)) + + if (between(a, b, c) || between(a, b, d) || + between(c, d, a) || between(c, d, b)) return true; - else - return false; + + return false; } static bool vequal(const int* a, const int* b) @@ -983,7 +984,7 @@ static bool removeVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short /// @par /// /// @note If the mesh data is to be used to construct a Detour navigation mesh, then the upper -/// limit must be retricted to <= #DT_VERTS_PER_POLYGON. +/// limit must be restricted to <= #DT_VERTS_PER_POLYGON. /// /// @see rcAllocPolyMesh, rcContourSet, rcPolyMesh, rcConfig bool rcBuildPolyMesh(rcContext* ctx, const rcContourSet& cset, const int nvp, rcPolyMesh& mesh) diff --git a/thirdparty/recastnavigation/Recast/Source/RecastMeshDetail.cpp b/thirdparty/recastnavigation/Recast/Source/RecastMeshDetail.cpp index 40f5b8c60b..957245cfb6 100644 --- a/thirdparty/recastnavigation/Recast/Source/RecastMeshDetail.cpp +++ b/thirdparty/recastnavigation/Recast/Source/RecastMeshDetail.cpp @@ -589,8 +589,8 @@ static void triangulateHull(const int /*nverts*/, const float* verts, const int // Triangulate the polygon by moving left or right, // depending on which triangle has shorter perimeter. - // This heuristic was chose emprically, since it seems - // handle tesselated straight edges well. + // This heuristic was chose empirically, since it seems + // handle tessellated straight edges well. while (next(left, nhull) != right) { // Check to see if se should advance left or right. @@ -634,6 +634,40 @@ inline float getJitterY(const int i) return (((i * 0xd8163841) & 0xffff) / 65535.0f * 2.0f) - 1.0f; } +static bool onHull(int a, int b, int nhull, int* hull) +{ + // All internal sampled points come after the hull so we can early out for those. + if (a >= nhull || b >= nhull) + return false; + + for (int j = nhull - 1, i = 0; i < nhull; j = i++) + { + if (a == hull[j] && b == hull[i]) + return true; + } + + return false; +} + +// Find edges that lie on hull and mark them as such. +static void setTriFlags(rcIntArray& tris, int nhull, int* hull) +{ + // Matches DT_DETAIL_EDGE_BOUNDARY + const int DETAIL_EDGE_BOUNDARY = 0x1; + + for (int i = 0; i < tris.size(); i += 4) + { + int a = tris[i + 0]; + int b = tris[i + 1]; + int c = tris[i + 2]; + unsigned short flags = 0; + flags |= (onHull(a, b, nhull, hull) ? DETAIL_EDGE_BOUNDARY : 0) << 0; + flags |= (onHull(b, c, nhull, hull) ? DETAIL_EDGE_BOUNDARY : 0) << 2; + flags |= (onHull(c, a, nhull, hull) ? DETAIL_EDGE_BOUNDARY : 0) << 4; + tris[i + 3] = (int)flags; + } +} + static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, const float sampleDist, const float sampleMaxError, const int heightSearchRadius, const rcCompactHeightfield& chf, @@ -771,6 +805,7 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, if (minExtent < sampleDist*2) { triangulateHull(nverts, verts, nhull, hull, nin, tris); + setTriFlags(tris, nhull, hull); return true; } @@ -875,6 +910,8 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, tris.resize(MAX_TRIS*4); ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Shrinking triangle count from %d to max %d.", ntris, MAX_TRIS); } + + setTriFlags(tris, nhull, hull); return true; } @@ -1137,31 +1174,6 @@ static void getHeightData(rcContext* ctx, const rcCompactHeightfield& chf, } } -static unsigned char getEdgeFlags(const float* va, const float* vb, - const float* vpoly, const int npoly) -{ - // The flag returned by this function matches dtDetailTriEdgeFlags in Detour. - // Figure out if edge (va,vb) is part of the polygon boundary. - static const float thrSqr = rcSqr(0.001f); - for (int i = 0, j = npoly-1; i < npoly; j=i++) - { - if (distancePtSeg2d(va, &vpoly[j*3], &vpoly[i*3]) < thrSqr && - distancePtSeg2d(vb, &vpoly[j*3], &vpoly[i*3]) < thrSqr) - return 1; - } - return 0; -} - -static unsigned char getTriFlags(const float* va, const float* vb, const float* vc, - const float* vpoly, const int npoly) -{ - unsigned char flags = 0; - flags |= getEdgeFlags(va,vb,vpoly,npoly) << 0; - flags |= getEdgeFlags(vb,vc,vpoly,npoly) << 2; - flags |= getEdgeFlags(vc,va,vpoly,npoly) << 4; - return flags; -} - /// @par /// /// See the #rcConfig documentation for more information on the configuration parameters. @@ -1377,7 +1389,7 @@ bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompa dmesh.tris[dmesh.ntris*4+0] = (unsigned char)t[0]; dmesh.tris[dmesh.ntris*4+1] = (unsigned char)t[1]; dmesh.tris[dmesh.ntris*4+2] = (unsigned char)t[2]; - dmesh.tris[dmesh.ntris*4+3] = getTriFlags(&verts[t[0]*3], &verts[t[1]*3], &verts[t[2]*3], poly, npoly); + dmesh.tris[dmesh.ntris*4+3] = (unsigned char)t[3]; dmesh.ntris++; } } diff --git a/thirdparty/recastnavigation/Recast/Source/RecastRasterization.cpp b/thirdparty/recastnavigation/Recast/Source/RecastRasterization.cpp index 2a4f619fb8..d54ae7202b 100644 --- a/thirdparty/recastnavigation/Recast/Source/RecastRasterization.cpp +++ b/thirdparty/recastnavigation/Recast/Source/RecastRasterization.cpp @@ -17,7 +17,6 @@ // #include -#include #include "Recast.h" #include "RecastAlloc.h" #include "RecastAssert.h" @@ -40,12 +39,12 @@ static bool overlapBounds(const float* aMin, const float* aMax, const float* bMi /// Allocates a new span in the heightfield. /// Use a memory pool and free list to minimize actual allocations. /// -/// @param[in] hf The heightfield +/// @param[in] heightfield The heightfield /// @returns A pointer to the allocated or re-used span memory. -static rcSpan* allocSpan(rcHeightfield& hf) +static rcSpan* allocSpan(rcHeightfield& heightfield) { // If necessary, allocate new page and update the freelist. - if (hf.freelist == NULL || hf.freelist->next == NULL) + if (heightfield.freelist == NULL || heightfield.freelist->next == NULL) { // Create new page. // Allocate memory for the new pool. @@ -56,11 +55,11 @@ static rcSpan* allocSpan(rcHeightfield& hf) } // Add the pool into the list of pools. - spanPool->next = hf.pools; - hf.pools = spanPool; + spanPool->next = heightfield.pools; + heightfield.pools = spanPool; // Add new spans to the free list. - rcSpan* freeList = hf.freelist; + rcSpan* freeList = heightfield.freelist; rcSpan* head = &spanPool->items[0]; rcSpan* it = &spanPool->items[RC_SPANS_PER_POOL]; do @@ -70,46 +69,46 @@ static rcSpan* allocSpan(rcHeightfield& hf) freeList = it; } while (it != head); - hf.freelist = it; + heightfield.freelist = it; } // Pop item from the front of the free list. - rcSpan* newSpan = hf.freelist; - hf.freelist = hf.freelist->next; + rcSpan* newSpan = heightfield.freelist; + heightfield.freelist = heightfield.freelist->next; return newSpan; } /// Releases the memory used by the span back to the heightfield, so it can be re-used for new spans. -/// @param[in] hf The heightfield. +/// @param[in] heightfield The heightfield. /// @param[in] span A pointer to the span to free -static void freeSpan(rcHeightfield& hf, rcSpan* span) +static void freeSpan(rcHeightfield& heightfield, rcSpan* span) { if (span == NULL) { return; } // Add the span to the front of the free list. - span->next = hf.freelist; - hf.freelist = span; + span->next = heightfield.freelist; + heightfield.freelist = span; } /// Adds a span to the heightfield. If the new span overlaps existing spans, /// it will merge the new span with the existing ones. /// -/// @param[in] hf Heightfield to add spans to +/// @param[in] heightfield Heightfield to add spans to /// @param[in] x The new span's column cell x index /// @param[in] z The new span's column cell z index /// @param[in] min The new span's minimum cell index /// @param[in] max The new span's maximum cell index /// @param[in] areaID The new span's area type ID /// @param[in] flagMergeThreshold How close two spans maximum extents need to be to merge area type IDs -static bool addSpan(rcHeightfield& hf, +static bool addSpan(rcHeightfield& heightfield, const int x, const int z, const unsigned short min, const unsigned short max, const unsigned char areaID, const int flagMergeThreshold) { // Create the new span. - rcSpan* newSpan = allocSpan(hf); + rcSpan* newSpan = allocSpan(heightfield); if (newSpan == NULL) { return false; @@ -119,9 +118,9 @@ static bool addSpan(rcHeightfield& hf, newSpan->area = areaID; newSpan->next = NULL; - const int columnIndex = x + z * hf.width; + const int columnIndex = x + z * heightfield.width; rcSpan* previousSpan = NULL; - rcSpan* currentSpan = hf.spans[columnIndex]; + rcSpan* currentSpan = heightfield.spans[columnIndex]; // Insert the new span, possibly merging it with existing spans. while (currentSpan != NULL) @@ -160,14 +159,14 @@ static bool addSpan(rcHeightfield& hf, // Remove the current span since it's now merged with newSpan. // Keep going because there might be other overlapping spans that also need to be merged. rcSpan* next = currentSpan->next; - freeSpan(hf, currentSpan); + freeSpan(heightfield, currentSpan); if (previousSpan) { previousSpan->next = next; } else { - hf.spans[columnIndex] = next; + heightfield.spans[columnIndex] = next; } currentSpan = next; } @@ -182,8 +181,8 @@ static bool addSpan(rcHeightfield& hf, else { // This span should go before the others in the list - newSpan->next = hf.spans[columnIndex]; - hf.spans[columnIndex] = newSpan; + newSpan->next = heightfield.spans[columnIndex]; + heightfield.spans[columnIndex] = newSpan; } return true; @@ -296,17 +295,17 @@ static void dividePoly(const float* inVerts, int inVertsCount, /// @param[in] v1 Triangle vertex 1 /// @param[in] v2 Triangle vertex 2 /// @param[in] areaID The area ID to assign to the rasterized spans -/// @param[in] hf Heightfield to rasterize into -/// @param[in] hfBBMin The min extents of the heightfield bounding box -/// @param[in] hfBBMax The max extents of the heightfield bounding box +/// @param[in] heightfield Heightfield to rasterize into +/// @param[in] heightfieldBBMin The min extents of the heightfield bounding box +/// @param[in] heightfieldBBMax The max extents of the heightfield bounding box /// @param[in] cellSize The x and z axis size of a voxel in the heightfield /// @param[in] inverseCellSize 1 / cellSize /// @param[in] inverseCellHeight 1 / cellHeight /// @param[in] flagMergeThreshold The threshold in which area flags will be merged /// @returns true if the operation completes successfully. false if there was an error adding spans to the heightfield. static bool rasterizeTri(const float* v0, const float* v1, const float* v2, - const unsigned char areaID, rcHeightfield& hf, - const float* hfBBMin, const float* hfBBMax, + const unsigned char areaID, rcHeightfield& heightfield, + const float* heightfieldBBMin, const float* heightfieldBBMax, const float cellSize, const float inverseCellSize, const float inverseCellHeight, const int flagMergeThreshold) { @@ -322,18 +321,18 @@ static bool rasterizeTri(const float* v0, const float* v1, const float* v2, rcVmax(triBBMax, v2); // If the triangle does not touch the bounding box of the heightfield, skip the triangle. - if (!overlapBounds(triBBMin, triBBMax, hfBBMin, hfBBMax)) + if (!overlapBounds(triBBMin, triBBMax, heightfieldBBMin, heightfieldBBMax)) { return true; } - const int w = hf.width; - const int h = hf.height; - const float by = hfBBMax[1] - hfBBMin[1]; + const int w = heightfield.width; + const int h = heightfield.height; + const float by = heightfieldBBMax[1] - heightfieldBBMin[1]; // Calculate the footprint of the triangle on the grid's z-axis - int z0 = (int)((triBBMin[2] - hfBBMin[2]) * inverseCellSize); - int z1 = (int)((triBBMax[2] - hfBBMin[2]) * inverseCellSize); + int z0 = (int)((triBBMin[2] - heightfieldBBMin[2]) * inverseCellSize); + int z1 = (int)((triBBMax[2] - heightfieldBBMin[2]) * inverseCellSize); // use -1 rather than 0 to cut the polygon properly at the start of the tile z0 = rcClamp(z0, -1, h - 1); @@ -355,7 +354,7 @@ static bool rasterizeTri(const float* v0, const float* v1, const float* v2, for (int z = z0; z <= z1; ++z) { // Clip polygon to row. Store the remaining polygon as well - const float cellZ = hfBBMin[2] + (float)z * cellSize; + const float cellZ = heightfieldBBMin[2] + (float)z * cellSize; dividePoly(in, nvIn, inRow, &nvRow, p1, &nvIn, cellZ + cellSize, RC_AXIS_Z); rcSwap(in, p1); @@ -382,8 +381,8 @@ static bool rasterizeTri(const float* v0, const float* v1, const float* v2, maxX = inRow[vert * 3]; } } - int x0 = (int)((minX - hfBBMin[0]) * inverseCellSize); - int x1 = (int)((maxX - hfBBMin[0]) * inverseCellSize); + int x0 = (int)((minX - heightfieldBBMin[0]) * inverseCellSize); + int x1 = (int)((maxX - heightfieldBBMin[0]) * inverseCellSize); if (x1 < 0 || x0 >= w) { continue; @@ -397,7 +396,7 @@ static bool rasterizeTri(const float* v0, const float* v1, const float* v2, for (int x = x0; x <= x1; ++x) { // Clip polygon to column. store the remaining polygon as well - const float cx = hfBBMin[0] + (float)x * cellSize; + const float cx = heightfieldBBMin[0] + (float)x * cellSize; dividePoly(inRow, nv2, p1, &nv, p2, &nv2, cx + cellSize, RC_AXIS_X); rcSwap(inRow, p2); @@ -418,8 +417,8 @@ static bool rasterizeTri(const float* v0, const float* v1, const float* v2, spanMin = rcMin(spanMin, p1[vert * 3 + 1]); spanMax = rcMax(spanMax, p1[vert * 3 + 1]); } - spanMin -= hfBBMin[1]; - spanMax -= hfBBMin[1]; + spanMin -= heightfieldBBMin[1]; + spanMax -= heightfieldBBMin[1]; // Skip the span if it's completely outside the heightfield bounding box if (spanMax < 0.0f) @@ -445,7 +444,7 @@ static bool rasterizeTri(const float* v0, const float* v1, const float* v2, unsigned short spanMinCellIndex = (unsigned short)rcClamp((int)floorf(spanMin * inverseCellHeight), 0, RC_SPAN_MAX_HEIGHT); unsigned short spanMaxCellIndex = (unsigned short)rcClamp((int)ceilf(spanMax * inverseCellHeight), (int)spanMinCellIndex + 1, RC_SPAN_MAX_HEIGHT); - if (!addSpan(hf, x, z, spanMinCellIndex, spanMaxCellIndex, areaID, flagMergeThreshold)) + if (!addSpan(heightfield, x, z, spanMinCellIndex, spanMaxCellIndex, areaID, flagMergeThreshold)) { return false; } diff --git a/thirdparty/recastnavigation/Recast/Source/RecastRegion.cpp b/thirdparty/recastnavigation/Recast/Source/RecastRegion.cpp index 4a7e841a92..684987ef7e 100644 --- a/thirdparty/recastnavigation/Recast/Source/RecastRegion.cpp +++ b/thirdparty/recastnavigation/Recast/Source/RecastRegion.cpp @@ -1072,12 +1072,14 @@ static bool mergeAndFilterLayerRegions(rcContext* ctx, int minRegionArea, for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) { const rcCompactSpan& s = chf.spans[i]; + const unsigned char area = chf.areas[i]; const unsigned short ri = srcReg[i]; if (ri == 0 || ri >= nreg) continue; rcRegion& reg = regions[ri]; reg.spanCount++; - + reg.areaType = area; + reg.ymin = rcMin(reg.ymin, s.y); reg.ymax = rcMax(reg.ymax, s.y); @@ -1157,6 +1159,9 @@ static bool mergeAndFilterLayerRegions(rcContext* ctx, int minRegionArea, // Skip already visited. if (regn.id != 0) continue; + // Skip if different area type, do not connect regions with different area type. + if (reg.areaType != regn.areaType) + continue; // Skip if the neighbour is overlapping root region. bool overlap = false; for (int k = 0; k < root.floors.size(); k++) @@ -1339,7 +1344,7 @@ struct rcSweepSpan /// re-assigned to the zero (null) region. /// /// Partitioning can result in smaller than necessary regions. @p mergeRegionArea helps -/// reduce unecessarily small regions. +/// reduce unnecessarily small regions. /// /// See the #rcConfig documentation for more information on the configuration parameters. /// @@ -1512,7 +1517,7 @@ bool rcBuildRegionsMonotone(rcContext* ctx, rcCompactHeightfield& chf, /// re-assigned to the zero (null) region. /// /// Watershed partitioning can result in smaller than necessary regions, especially in diagonal corridors. -/// @p mergeRegionArea helps reduce unecessarily small regions. +/// @p mergeRegionArea helps reduce unnecessarily small regions. /// /// See the #rcConfig documentation for more information on the configuration parameters. /// @@ -1637,7 +1642,7 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf, { rcScopedTimer timerFilter(ctx, RC_TIMER_BUILD_REGIONS_FILTER); - // Merge regions and filter out smalle regions. + // Merge regions and filter out small regions. rcIntArray overlaps; chf.maxRegions = regionId; if (!mergeAndFilterRegions(ctx, minRegionArea, mergeRegionArea, chf.maxRegions, chf, srcReg, overlaps)) -- 2.34.1