From 938681a52e005c721f71c6933f71baf085bbf8e5 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 7 Nov 2026 17:08:57 -0800 Subject: [PATCH] 2d hull --- include/box3d/collision.h | 15 ++- samples/sample_collision.cpp | 25 ++++- src/convex_manifold.c | 2 +- src/hull.c | 186 ++++++++++++++++++++++++----------- src/mesh_contact.c | 113 ++++++++++++++++++++- test/test_hull.c | 155 +++++++++++++++++++++++++++-- 6 files changed, 422 insertions(+), 74 deletions(-) diff --git a/include/box3d/collision.h b/include/box3d/collision.h index d5e2e83..76e3cce 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -692,8 +692,21 @@ typedef struct b3Point2D int originalIndex; // original index of the associated contact point } b3Point2D; +/// Options for how a 2D hull is built. +typedef enum b3Hull2DFlags +{ + /// Keep collinear points. + b3_hull2DKeepCollinear = 0x01, + + /// Only merge identical points. + b3_hull2DExactMerge = 0x02, + + /// Order the output points by the input order. + b3_hull2DIndexOrder = 0x04, +} b3Hull2DFlags; + /// Compute a 2D hull. Used internally for contact manifold simplification. Here for testing. -B3_API int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull ); +B3_API int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull, int flags ); /// Simplify a 2D hull. Used internally for contact manifold simplification. Here for testing. B3_API int b3SimplifyHull2D( b3Point2D* hull, int count, int target ); diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index 370315c..6251cd2 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -2900,6 +2900,7 @@ public: e_random = 0, e_circle = 1, e_sliver = 2, + e_grid = 3, }; static Sample* Create( SampleContext* context ) @@ -2923,6 +2924,7 @@ public: m_pointCount = 20; m_drawScale = 1.0f; m_drawLabels = true; + m_hullFlags = 0; Generate(); } @@ -2961,6 +2963,20 @@ public: m_inputPoints[i].originalIndex = i; } } + else if ( m_sourceMode == e_grid ) + { + // Every border point is collinear with its neighbors, so the hull keeps them only with + // the collinear flag + m_drawScale = 1.0f; + m_inputCount = 36; + + for ( int i = 0; i < m_inputCount; ++i ) + { + m_inputPoints[i].p = { -5.0f + 2.0f * (float)( i % 6 ), -5.0f + 2.0f * (float)( i / 6 ) }; + m_inputPoints[i].separation = 0.0f; + m_inputPoints[i].originalIndex = i; + } + } else { // A near degenerate sliver with a duplicate pair. Welding and collinear removal have @@ -2998,7 +3014,7 @@ public: scratch[i] = m_inputPoints[i]; } - m_hullCount = b3Hull2D( scratch, m_inputCount, m_hullPoints ); + m_hullCount = b3Hull2D( scratch, m_inputCount, m_hullPoints, m_hullFlags ); for ( int i = 0; i < m_hullCount; ++i ) { @@ -3010,13 +3026,13 @@ public: bool DrawControls() override { - const char* modes[] = { "Random", "Circle", "Sliver" }; + const char* modes[] = { "Random", "Circle", "Sliver", "Grid" }; if ( ImGui::Combo( "Source", &m_sourceMode, modes, IM_ARRAYSIZE( modes ) ) ) { Generate(); } - if ( m_sourceMode != e_sliver ) + if ( m_sourceMode == e_random || m_sourceMode == e_circle ) { if ( ImGui::SliderInt( "Point Count", &m_pointCount, 4, m_pointCapacity ) ) { @@ -3033,6 +3049,8 @@ public: } ImGui::SliderInt( "Target Count", &m_targetCount, 3, m_inputCount ); + ImGui::CheckboxFlags( "Keep Collinear", &m_hullFlags, b3_hull2DKeepCollinear ); + ImGui::CheckboxFlags( "Exact Merge", &m_hullFlags, b3_hull2DExactMerge ); ImGui::Checkbox( "Draw Labels", &m_drawLabels ); return true; @@ -3110,6 +3128,7 @@ public: int m_simplifiedCount; int m_targetCount; int m_pointCount; + int m_hullFlags; bool m_drawLabels; }; diff --git a/src/convex_manifold.c b/src/convex_manifold.c index 6f84f44..85337c7 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -990,7 +990,7 @@ static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3L } b3Point2D hull[2 * B3_MAX_CLIP_POINTS]; - int hullCount = b3Hull2D( pts, count, hull ); + int hullCount = b3Hull2D( pts, count, hull, 0 ); int finalCount = b3SimplifyHull2D( hull, hullCount, target ); B3_ASSERT( 0 < finalCount && finalCount <= target ); diff --git a/src/hull.c b/src/hull.c index b0392d8..91004d3 100644 --- a/src/hull.c +++ b/src/hull.c @@ -7,6 +7,7 @@ #include "algorithm.h" #include "math_internal.h" +#include "qsort.h" #include "shape.h" #include "simd.h" @@ -3098,20 +3099,20 @@ int b3SimplifyHull2D( b3Point2D* hull, int count1, int target ) return count2; } -// Sort comparator: by x, then by y. Insertion since n is typically 20 or less. -static void b3Sort2D( b3Point2D* pts, int count ) +// todo comment +static inline bool b3IsPoint2DLess( const b3Point2D* a, const b3Point2D* b ) { - for ( int i = 1; i < count; ++i ) + if ( a->p.x != b->p.x ) { - b3Point2D base = pts[i]; - int j = i - 1; - while ( j >= 0 && ( pts[j].p.x > base.p.x || ( pts[j].p.x == base.p.x && pts[j].p.y > base.p.y ) ) ) - { - pts[j + 1] = pts[j]; - j -= 1; - } - pts[j + 1] = base; + return a->p.x < b->p.x; } + + if ( a->p.y != b->p.y ) + { + return a->p.y < b->p.y; + } + + return a->originalIndex < b->originalIndex; } // Welds points that have been sorted O(n) time @@ -3148,79 +3149,144 @@ static int b3Weld2D( b3Point2D* pts, int count1 ) return count2; } +// todo comment +static int b3MergeExact2D( b3Point2D* pts, int count1 ) +{ + float separationTol = B3_LINEAR_SLOP; + + int count2 = 0; + for ( int i = 0; i < count1; i++ ) + { + b3Point2D point = pts[i]; + if ( count2 > 0 && pts[count2 - 1].p.x == point.p.x && pts[count2 - 1].p.y == point.p.y ) + { + if ( point.separation < pts[count2 - 1].separation - separationTol ) + { + pts[count2 - 1] = point; + } + } + else + { + pts[count2] = point; + count2 += 1; + } + } + + return count2; +} + // Andrew's monotone chain convex hull algorithm // hull must have space for 2 * count1 points -int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull ) +int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull, int flags ) { - int count1 = count; - - if ( count1 <= 0 ) + if ( count <= 0 ) { return 0; } - if ( count1 == 1 ) - { - hull[0] = pts[0]; - return 1; - } +#define LESS( i, j ) b3IsPoint2DLess( pts + (int)( i ), pts + (int)( j ) ) +#define SWAP( i, j ) \ + do \ + { \ + b3Point2D swapPoint = pts[(int)( i )]; \ + pts[(int)( i )] = pts[(int)( j )]; \ + pts[(int)( j )] = swapPoint; \ + } \ + while ( 0 ) - b3Sort2D( pts, count1 ); - count1 = b3Weld2D( pts, count1 ); + QSORT( count, LESS, SWAP ); - if ( count1 == 1 ) - { - hull[0] = pts[0]; - return 1; - } +#undef LESS +#undef SWAP - if ( count1 == 2 ) - { - hull[0] = pts[0]; - hull[1] = pts[1]; - return 2; - } + int count1 = ( flags & b3_hull2DExactMerge ) ? b3MergeExact2D( pts, count ) : b3Weld2D( pts, count ); + bool keepCollinear = ( flags & b3_hull2DKeepCollinear ) != 0; int count2 = 0; - - for ( int i = 0; i < count1; i++ ) + if ( count1 <= 2 ) { - while ( count2 >= 2 ) + for ( int i = 0; i < count1; i++ ) { - float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); - if ( area > 0.0f ) - { - break; - } - - count2 -= 1; + hull[i] = pts[i]; } - B3_VALIDATE( count2 < 2 * count1 ); - hull[count2] = pts[i]; - count2 += 1; + count2 = count1; } - - int lowerCount = count2 + 1; - - for ( int i = count1 - 2; i >= 0; i-- ) + else { - while ( count2 >= lowerCount ) + for ( int i = 0; i < count1; i++ ) { - float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); - if ( area > 0.0f ) + while ( count2 >= 2 ) { - break; + float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); + if ( area > 0.0f || ( keepCollinear && area == 0.0f ) ) + { + break; + } + + count2 -= 1; } - count2 -= 1; + B3_VALIDATE( count2 < 2 * count1 ); + hull[count2] = pts[i]; + count2 += 1; } - B3_VALIDATE( count2 < 2 * count1 ); - hull[count2] = pts[i]; - count2 += 1; + int lowerCount = count2 + 1; + + // todo comment + int lowerIndex = count2 - 2; + + for ( int i = count1 - 2; i >= 0; i-- ) + { + if ( lowerIndex >= 0 && hull[lowerIndex].p.x == pts[i].p.x && hull[lowerIndex].p.y == pts[i].p.y ) + { + lowerIndex -= 1; + if ( i > 0 ) + { + continue; + } + } + + while ( count2 >= lowerCount ) + { + float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); + if ( area > 0.0f || ( keepCollinear && area == 0.0f ) ) + { + break; + } + + count2 -= 1; + } + + B3_VALIDATE( count2 < 2 * count1 ); + hull[count2] = pts[i]; + count2 += 1; + } + + B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex ); + count2 -= 1; } - B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex ); - return count2 - 1; + B3_VALIDATE( count2 <= count1 ); + + if ( flags & b3_hull2DIndexOrder ) + { +#define LESS( i, j ) ( hull[(int)( i )].originalIndex < hull[(int)( j )].originalIndex ) +#define SWAP( i, j ) \ + do \ + { \ + b3Point2D swapPoint = hull[(int)( i )]; \ + hull[(int)( i )] = hull[(int)( j )]; \ + hull[(int)( j )] = swapPoint; \ + } \ + while ( 0 ) + + QSORT( count2, LESS, SWAP ); + +#undef LESS +#undef SWAP + } + + return count2; } diff --git a/src/mesh_contact.c b/src/mesh_contact.c index bb3534a..eb07d03 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -349,6 +349,87 @@ static int b3CullPoints( b3Point2D* points, int count ) return 4; } +// todo comment +#define B3_CULL_HULL_THRESHOLD 64 + +// todo comment +static int b3DropInteriorPoints( b3Point2D* points, int count, float margin ) +{ + int extremes[8] = { 0 }; + float bests[8] = { + points[0].p.x, + points[0].p.x + points[0].p.y, + points[0].p.y, + points[0].p.y - points[0].p.x, + -points[0].p.x, + -points[0].p.x - points[0].p.y, + -points[0].p.y, + points[0].p.x - points[0].p.y, + }; + + for ( int i = 1; i < count; ++i ) + { + b3Vec2 p = points[i].p; + float values[8] = { p.x, p.x + p.y, p.y, p.y - p.x, -p.x, -p.x - p.y, -p.y, p.x - p.y }; + for ( int k = 0; k < 8; ++k ) + { + if ( values[k] > bests[k] ) + { + bests[k] = values[k]; + extremes[k] = i; + } + } + } + + // todo comment + b3Vec2 normals[8]; + float offsets[8]; + int edgeCount = 0; + for ( int k = 0; k < 8; ++k ) + { + b3Vec2 a = points[extremes[k]].p; + b3Vec2 b = points[extremes[( k + 1 ) & 7]].p; + b3Vec2 normal = { a.y - b.y, b.x - a.x }; + float length = b3Length2( normal ); + if ( length == 0.0f ) + { + continue; + } + + normals[edgeCount] = normal; + offsets[edgeCount] = b3Dot2( normal, a ) + margin * length; + edgeCount += 1; + } + + if ( edgeCount < 3 ) + { + return count; + } + + int keptCount = 0; + for ( int i = 0; i < count; ++i ) + { + b3Vec2 p = points[i].p; + bool interior = true; + for ( int k = 0; k < edgeCount; ++k ) + { + if ( b3Dot2( normals[k], p ) <= offsets[k] ) + { + interior = false; + break; + } + } + + if ( interior == false ) + { + points[keptCount] = points[i]; + keptCount += 1; + } + } + + return keptCount; +} + #endif static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 normal, b3Arena arena ) @@ -374,10 +455,38 @@ static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 nor #if B3_MAX_MANIFOLD_POINTS == 4 b3Point2D* hull = pts; - int count2 = b3CullPoints( pts, count1 ); + int count2; + if ( count1 <= B3_CULL_HULL_THRESHOLD ) + { + count2 = b3CullPoints( pts, count1 ); + } + else + { + // todo comment + b3Point2D deepest = pts[0]; + for ( int i = 1; i < count1; ++i ) + { + if ( pts[i].separation < deepest.separation ) + { + deepest = pts[i]; + } + } + + int keptCount = b3DropInteriorPoints( pts, count1, 0.25f * B3_LINEAR_SLOP ); + hull = b3Bump( &arena, 2 * keptCount * sizeof( b3Point2D ) ); + int flags = b3_hull2DKeepCollinear | b3_hull2DExactMerge | b3_hull2DIndexOrder; + int hullCount = b3Hull2D( pts, keptCount, hull, flags ); + count2 = b3CullPoints( hull, hullCount ); + + // todo comment + if ( count2 == 1 ) + { + hull[0] = deepest; + } + } #else b3Point2D* hull = b3Bump( &arena, 2 * count1 * sizeof( b3Point2D ) ); - int hullCount = b3Hull2D( pts, count1, hull ); + int hullCount = b3Hull2D( pts, count1, hull, 0 ); int count2 = b3SimplifyHull2D( hull, hullCount, B3_MAX_MANIFOLD_POINTS ); #endif B3_ASSERT( count2 <= B3_MAX_MANIFOLD_POINTS ); diff --git a/test/test_hull.c b/test/test_hull.c index d2a27be..7696f8c 100644 --- a/test/test_hull.c +++ b/test/test_hull.c @@ -580,7 +580,7 @@ static int Hull2DSquareTest( void ) b3Point2D hull[16]; FillPoints2D( pts, input, 8 ); - int count = b3Hull2D( pts, 8, hull ); + int count = b3Hull2D( pts, 8, hull, 0 ); ENSURE( count == 4 ); // The reduction code assumes counter clockwise winding @@ -616,7 +616,7 @@ static int Hull2DCollinearTest( void ) b3Point2D hull[10]; FillPoints2D( pts, input, 5 ); - ENSURE( b3Hull2D( pts, 5, hull ) == 2 ); + ENSURE( b3Hull2D( pts, 5, hull, 0 ) == 2 ); return 0; } @@ -635,7 +635,7 @@ static int Hull2DWeldTest( void ) b3Point2D hull[10]; FillPoints2D( pts, input, 5 ); - int count = b3Hull2D( pts, 5, hull ); + int count = b3Hull2D( pts, 5, hull, 0 ); ENSURE( count == 4 ); for ( int i = 0; i < count; ++i ) @@ -659,7 +659,7 @@ static int SimplifyHull2DTargetTest( void ) pts[i].originalIndex = i; } - int hullCount = b3Hull2D( pts, 20, hull ); + int hullCount = b3Hull2D( pts, 20, hull, 0 ); ENSURE( hullCount == 20 ); int count = b3SimplifyHull2D( hull, hullCount, 6 ); @@ -682,7 +682,7 @@ static int SimplifyHull2DNoopTest( void ) b3Point2D hull[8]; FillPoints2D( pts, input, 4 ); - int hullCount = b3Hull2D( pts, 4, hull ); + int hullCount = b3Hull2D( pts, 4, hull, 0 ); ENSURE( hullCount == 4 ); ENSURE( b3SimplifyHull2D( hull, hullCount, 8 ) == 4 ); @@ -706,7 +706,7 @@ static int Hull2DOrderTest( void ) b3Point2D hull[16]; FillPoints2D( pts, input, 8 ); - int count1 = b3Hull2D( pts, 8, hull ); + int count1 = b3Hull2D( pts, 8, hull, 0 ); ENSURE( count1 == 7 ); bool onHull1[8] = { false }; @@ -724,7 +724,7 @@ static int Hull2DOrderTest( void ) shuffled[i].originalIndex = i; } - int count2 = b3Hull2D( shuffled, 8, shuffledHull ); + int count2 = b3Hull2D( shuffled, 8, shuffledHull, 0 ); ENSURE( count2 == count1 ); bool onHull2[8] = { false }; @@ -741,6 +741,143 @@ static int Hull2DOrderTest( void ) return 0; } +// Float rounding can call the middle of three nearly collinear points a left turn in both chains. +// Listing it twice gave the mesh manifold a duplicate point. +static int Hull2DNearCollinearTest( void ) +{ + b3Vec2 input[3] = { + { 0.42742428183555603f, -0.9914372563362122f }, + { 0.8582707643508911f, -0.7915351390838623f }, + { 1.5856003761291504f, -0.45407217741012573f }, + }; + + b3Point2D pts[3]; + b3Point2D hull[6]; + FillPoints2D( pts, input, 3 ); + + int count = b3Hull2D( pts, 3, hull, 0 ); + ENSURE( 2 <= count && count <= 3 ); + + for ( int i = 0; i < count; ++i ) + { + for ( int j = i + 1; j < count; ++j ) + { + ENSURE( hull[i].originalIndex != hull[j].originalIndex ); + } + } + + return 0; +} + +// A 3 by 3 grid has three points on each side. Only the corners are strict hull vertices. +static int Hull2DKeepCollinearTest( void ) +{ + b3Vec2 input[9]; + for ( int i = 0; i < 9; ++i ) + { + input[i] = (b3Vec2){ (float)( i % 3 ), (float)( i / 3 ) }; + } + + b3Point2D pts[9]; + b3Point2D hull[18]; + FillPoints2D( pts, input, 9 ); + ENSURE( b3Hull2D( pts, 9, hull, 0 ) == 4 ); + + FillPoints2D( pts, input, 9 ); + int count = b3Hull2D( pts, 9, hull, b3_hull2DKeepCollinear ); + ENSURE( count == 8 ); + ENSURE( Hull2DArea( hull, count ) > 0.0f ); + + for ( int i = 0; i < count; ++i ) + { + ENSURE( hull[i].originalIndex != 4 ); + } + + // All collinear keeps every point + b3Vec2 line[4] = { { 0.0f, 0.0f }, { 1.0f, 0.0f }, { 2.0f, 0.0f }, { 3.0f, 0.0f } }; + FillPoints2D( pts, line, 4 ); + ENSURE( b3Hull2D( pts, 4, hull, b3_hull2DKeepCollinear ) == 4 ); + + return 0; +} + +// Coincident points under the exact merge: the earlier point survives unless a later one is deeper by +// more than the linear slop. The default weld always keeps the earlier point. +static int Hull2DExactMergeTest( void ) +{ + b3Vec2 input[5] = { + { -1.0f, -1.0f }, { 1.0f, -1.0f }, { 1.0f, 1.0f }, { -1.0f, 1.0f }, { -1.0f, -1.0f }, + }; + + float depths[2] = { 2.0f * B3_LINEAR_SLOP, 0.5f * B3_LINEAR_SLOP }; + int expected[2] = { 4, 0 }; + + for ( int k = 0; k < 2; ++k ) + { + b3Point2D pts[5]; + b3Point2D hull[10]; + FillPoints2D( pts, input, 5 ); + pts[4].separation = -depths[k]; + + int count = b3Hull2D( pts, 5, hull, b3_hull2DExactMerge ); + ENSURE( count == 4 ); + + int survivor = -1; + for ( int i = 0; i < count; ++i ) + { + if ( hull[i].originalIndex == 0 || hull[i].originalIndex == 4 ) + { + survivor = hull[i].originalIndex; + } + } + ENSURE( survivor == expected[k] ); + + FillPoints2D( pts, input, 5 ); + pts[4].separation = -depths[k]; + count = b3Hull2D( pts, 5, hull, 0 ); + ENSURE( count == 4 ); + + for ( int i = 0; i < count; ++i ) + { + ENSURE( hull[i].originalIndex != 4 ); + } + } + + return 0; +} + +// The index order flag returns the same points sorted by input index +static int Hull2DIndexOrderTest( void ) +{ + b3Vec2 input[8] = { + { 1.0f, 2.0f }, { -2.5f, 0.4f }, { 2.0f, -1.0f }, { 0.2f, 0.1f }, + { -2.0f, -1.0f }, { 2.5f, 0.5f }, { 0.0f, -1.5f }, { -1.0f, 1.8f }, + }; + + b3Point2D pts[8]; + b3Point2D hull[16]; + FillPoints2D( pts, input, 8 ); + int count1 = b3Hull2D( pts, 8, hull, 0 ); + + bool onHull[8] = { false }; + for ( int i = 0; i < count1; ++i ) + { + onHull[hull[i].originalIndex] = true; + } + + FillPoints2D( pts, input, 8 ); + int count2 = b3Hull2D( pts, 8, hull, b3_hull2DIndexOrder ); + ENSURE( count2 == count1 ); + + for ( int i = 0; i < count2; ++i ) + { + ENSURE( onHull[hull[i].originalIndex] ); + ENSURE( i == 0 || hull[i - 1].originalIndex < hull[i].originalIndex ); + } + + return 0; +} + int HullTest( void ) { RUN_SUBTEST( CreateHullCubeTest ); @@ -759,6 +896,10 @@ int HullTest( void ) RUN_SUBTEST( Hull2DCollinearTest ); RUN_SUBTEST( Hull2DWeldTest ); RUN_SUBTEST( Hull2DOrderTest ); + RUN_SUBTEST( Hull2DNearCollinearTest ); + RUN_SUBTEST( Hull2DKeepCollinearTest ); + RUN_SUBTEST( Hull2DExactMergeTest ); + RUN_SUBTEST( Hull2DIndexOrderTest ); RUN_SUBTEST( SimplifyHull2DTargetTest ); RUN_SUBTEST( SimplifyHull2DNoopTest );