diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index 0b5f00295..9241152e4 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -2232,7 +2232,9 @@ class Manifold : public Sample m_smgcapCache2 = b2_emptyDistanceCache; m_transform = b2Transform_identity; - m_transform.p.x = 1.25f; + m_transform.p.x = 1.0f; + m_transform.p.y = 0.0f; + //m_transform.q = b2MakeRot( 0.5f * b2_pi ); m_angle = 0.0f; m_round = 0.1f; @@ -2382,7 +2384,6 @@ class Manifold : public Sample m_smgcapCache2 = b2_emptyDistanceCache; } -#if 0 // circle-circle { b2Circle circle1 = { { 0.0f, 0.0f }, 0.5f }; @@ -2461,31 +2462,30 @@ class Manifold : public Sample offset = b2Add( offset, increment ); } -#endif // capsule-capsule { - b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + b2Capsule capsule1 = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + b2Capsule capsule2 = { { 0.25f, 0.0f }, { 1.0f, 0.0 }, 0.1f }; b2Transform transform1 = { offset, b2Rot_identity }; b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; - b2Manifold m = b2CollideCapsules( &capsule, transform1, &capsule, transform2 ); + b2Manifold m = b2CollideCapsules( &capsule1, transform1, &capsule2, transform2 ); - b2Vec2 v1 = b2TransformPoint( transform1, capsule.center1 ); - b2Vec2 v2 = b2TransformPoint( transform1, capsule.center2 ); - g_draw.DrawSolidCapsule( v1, v2, capsule.radius, color1 ); + b2Vec2 v1 = b2TransformPoint( transform1, capsule1.center1 ); + b2Vec2 v2 = b2TransformPoint( transform1, capsule1.center2 ); + g_draw.DrawSolidCapsule( v1, v2, capsule1.radius, color1 ); - v1 = b2TransformPoint( transform2, capsule.center1 ); - v2 = b2TransformPoint( transform2, capsule.center2 ); - g_draw.DrawSolidCapsule( v1, v2, capsule.radius, color2 ); + v1 = b2TransformPoint( transform2, capsule2.center1 ); + v2 = b2TransformPoint( transform2, capsule2.center2 ); + g_draw.DrawSolidCapsule( v1, v2, capsule2.radius, color2 ); DrawManifold( &m, transform1.p, transform2.p ); offset = b2Add( offset, increment ); } -#if 0 // box-capsule { b2Capsule capsule = { { -0.4f, 0.0f }, { -0.1f, 0.0f }, 0.1f }; @@ -2780,7 +2780,6 @@ class Manifold : public Sample offset.x += 2.0f * increment.x; } -#endif } static Sample* Create( Settings& settings ) diff --git a/src/array.h b/src/array.h index e5543ddd7..e590d292f 100644 --- a/src/array.h +++ b/src/array.h @@ -46,10 +46,10 @@ // Inline array functions that need the type T to be defined #define B2_ARRAY_INLINE( T, PREFIX ) \ /* Resize */ \ - static inline void PREFIX##Array_Resize( PREFIX##Array* a, int count ) \ + static inline void PREFIX##Array_Resize( PREFIX##Array* a, int count ) \ { \ PREFIX##Array_Reserve( a, count ); \ - a->count = count; \ + a->count = count; \ } \ /* Get */ \ static inline T* PREFIX##Array_Get( PREFIX##Array* a, int index ) \ @@ -122,10 +122,12 @@ /* Create */ \ PREFIX##Array PREFIX##Array_Create( int capacity ) \ { \ - PREFIX##Array a; \ - a.data = b2Alloc( capacity * sizeof( T ) ); \ - a.count = 0; \ - a.capacity = capacity; \ + PREFIX##Array a = { 0 }; \ + if ( capacity > 0 ) \ + { \ + a.data = b2Alloc( capacity * sizeof( T ) ); \ + a.capacity = capacity; \ + } \ return a; \ } \ /* Reserve */ \ diff --git a/src/core.c b/src/core.c index 8ea885bd7..030fb7282 100644 --- a/src/core.c +++ b/src/core.c @@ -82,6 +82,11 @@ void b2SetAllocator( b2AllocFcn* allocFcn, b2FreeFcn* freeFcn ) void* b2Alloc( int size ) { + if (size == 0) + { + return NULL; + } + // This could cause some sharing issues, however Box2D rarely calls b2Alloc. atomic_fetch_add_explicit( &b2_byteCount, size, memory_order_relaxed ); diff --git a/src/manifold.c b/src/manifold.c index b7c0d0310..b66785d28 100644 --- a/src/manifold.c +++ b/src/manifold.c @@ -318,7 +318,9 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const float distanceSquared = b2DistanceSquared( closest1, closest2 ); b2Manifold manifold = { 0 }; - float radius = capsuleA->radius + capsuleB->radius; + float radiusA = capsuleA->radius; + float radiusB = capsuleB->radius; + float radius = radiusA + radiusB; float maxDistance = radius + b2_speculativeDistance; if ( distanceSquared > maxDistance * maxDistance ) @@ -326,49 +328,27 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const return manifold; } - b2Vec2 u1 = b2Normalize( d1 ); - b2Vec2 u2 = b2Normalize( d2 ); + float distance = sqrt( distanceSquared ); + + float length1, length2; + b2Vec2 u1 = b2GetLengthAndNormalize( &length1, d1 ); + b2Vec2 u2 = b2GetLengthAndNormalize( &length2, d2 ); // Does segment B project outside segment A? float fp2 = b2Dot( b2Sub( p2, p1 ), u1 ); float fq2 = b2Dot( b2Sub( q2, p1 ), u1 ); - bool outsideA = ( fp2 <= 0.0f && fq2 <= 0.0f ) || ( fp2 >= 1.0f && fq2 >= 1.0f ); + bool outsideA = ( fp2 <= 0.0f && fq2 <= 0.0f ) || ( fp2 >= length1 && fq2 >= length1 ); // Does segment A project outside segment B? float fp1 = b2Dot( b2Sub( p1, p2 ), u2 ); float fq1 = b2Dot( b2Sub( q1, p2 ), u2 ); - bool outsideB = ( fp1 <= 0.0f && fq1 <= 0.0f ) || ( fp1 >= 1.0f && fq1 >= 1.0f ); - - bool flip = false; - if ( outsideA && outsideB ) - { - // vertex-vertex collision, no clipping needed - - b2Vec2 normal = b2Sub( closest2, closest1 ); - if ( b2Dot( normal, normal ) > epsSqr ) - { - normal = b2Normalize( normal ); - } - else - { - normal = b2LeftPerp( u1 ); - } - - b2Vec2 c1 = b2MulAdd( closest1, capsuleA->radius, normal ); - b2Vec2 c2 = b2MulAdd( closest1, -capsuleB->radius, normal ); + bool outsideB = ( fp1 <= 0.0f && fq1 <= 0.0f ) || ( fp1 >= length2 && fq1 >= length2 ); - int i1 = f1 == 0.0f ? 0 : 1; - int i2 = f2 == 0.0f ? 0 : 1; - - manifold.normal = normal; - manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); - manifold.points[0].separation = sqrtf(distanceSquared) - radius; - manifold.points[0].id = B2_MAKE_ID( i1, i2 ); - manifold.pointCount = 1; - } - else + if ( outsideA == false && outsideB == false) { - // can clip + // attempt to clip + // this may yield contact points with excessive separation + // in that case the algorithm falls back to single point collision // find reference edge using SAT b2Vec2 normalA; @@ -381,7 +361,7 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const float s1p = ss1 < ss2 ? ss1 : ss2; float s1n = -ss1 < -ss2 ? -ss1 : -ss2; - if (s1p > s1n) + if ( s1p > s1n ) { separationA = s1p; } @@ -401,7 +381,7 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const float s1p = ss1 < ss2 ? ss1 : ss2; float s1n = -ss1 < -ss2 ? -ss1 : -ss2; - if (s1p > s1n) + if ( s1p > s1n ) { separationB = s1p; } @@ -412,7 +392,7 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const } } - if (separationA > separationB) + if ( separationA >= separationB ) { manifold.normal = normalA; @@ -420,49 +400,47 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const b2Vec2 cq = q2; // clip to p1 - if (fp2 < 0.0f && fq2 > 0.0f) + if ( fp2 < 0.0f && fq2 > 0.0f ) { cp = b2Lerp( p2, q2, ( 0.0f - fp2 ) / ( fq2 - fp2 ) ); } - else if (fq2 < 0.0f && fp2 > 0.0f) + else if ( fq2 < 0.0f && fp2 > 0.0f ) { cq = b2Lerp( q2, p2, ( 0.0f - fq2 ) / ( fp2 - fq2 ) ); } // clip to q1 - if (fp2 > 1.0f && fq2 < 1.0f) + if ( fp2 > length1 && fq2 < length1 ) { - cp = b2Lerp( q2, p2, ( fp2 - 1.0f ) / ( fp2 - fq2 ) ); + cp = b2Lerp( p2, q2, ( fp2 - length1 ) / ( fp2 - fq2 ) ); } - else if (fq2 > 1.0f && fp2 < 1.0f) + else if ( fq2 > length1 && fp2 < length1 ) { - cq = b2Lerp( p2, q2, ( fq2 - 1.0f ) / ( fq2 - fp2 ) ); + cq = b2Lerp( q2, p2, ( fq2 - length1 ) / ( fq2 - fp2 ) ); } float sp = b2Dot( b2Sub( cp, p1 ), normalA ); float sq = b2Dot( b2Sub( cq, p1 ), normalA ); + if ( sp <= distance + b2_linearSlop || sq <= distance + b2_linearSlop ) { - b2ManifoldPoint* mp = manifold.points + 0; - mp->anchorA = cp; + b2ManifoldPoint* mp; + mp = manifold.points + 0; + mp->anchorA = b2MulAdd( cp, 0.5f * ( radiusA - radiusB - sp ), normalA ); mp->separation = sp - radius; - mp->id = B2_MAKE_ID(0, 0); - } + mp->id = B2_MAKE_ID( 0, 0 ); - { - b2ManifoldPoint* mp = manifold.points + 1; - mp->anchorA = cq; + mp = manifold.points + 1; + mp->anchorA = b2MulAdd( cq, 0.5f * ( radiusA - radiusB - sq ), normalA ); mp->separation = sq - radius; mp->id = B2_MAKE_ID( 0, 1 ); + manifold.pointCount = 2; } - - manifold.pointCount = 2; } else { - flip = true; - - manifold.normal = normalB; + // normal always points from A to B + manifold.normal = b2Neg( normalB ); b2Vec2 cp = p1; b2Vec2 cq = q1; @@ -478,34 +456,58 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const } // clip to q2 - if ( fp1 > 1.0f && fq1 < 1.0f ) + if ( fp1 > length2 && fq1 < length2 ) { - cp = b2Lerp( q1, p1, ( fp1 - 1.0f ) / ( fp1 - fq1 ) ); + cp = b2Lerp( p1, q1, ( fp1 - length2 ) / ( fp1 - fq1 ) ); } - else if ( fq1 > 1.0f && fp1 < 1.0f ) + else if ( fq1 > length2 && fp1 < length2 ) { - cq = b2Lerp( p1, q1, ( fq1 - 1.0f ) / ( fq1 - fp1 ) ); + cq = b2Lerp( q1, p1, ( fq1 - length2 ) / ( fq1 - fp1 ) ); } float sp = b2Dot( b2Sub( cp, p2 ), normalB ); float sq = b2Dot( b2Sub( cq, p2 ), normalB ); + if ( sp <= distance + b2_linearSlop || sq <= distance + b2_linearSlop ) { - b2ManifoldPoint* mp = manifold.points + 0; - mp->anchorA = cp; + b2ManifoldPoint* mp; + mp = manifold.points + 0; + mp->anchorA = b2MulAdd( cp, 0.5f * ( radiusB - radiusA - sp ), normalB ); mp->separation = sp - radius; mp->id = B2_MAKE_ID( 0, 0 ); - } - - { - b2ManifoldPoint* mp = manifold.points + 1; - mp->anchorA = cq; + mp = manifold.points + 1; + mp->anchorA = b2MulAdd( cq, 0.5f * ( radiusB - radiusA - sq ), normalB ); mp->separation = sq - radius; mp->id = B2_MAKE_ID( 1, 0 ); + manifold.pointCount = 2; } + } + } - manifold.pointCount = 2; + if (manifold.pointCount == 0) + { + // single point collision + b2Vec2 normal = b2Sub( closest2, closest1 ); + if ( b2Dot( normal, normal ) > epsSqr ) + { + normal = b2Normalize( normal ); } + else + { + normal = b2LeftPerp( u1 ); + } + + b2Vec2 c1 = b2MulAdd( closest1, radiusA, normal ); + b2Vec2 c2 = b2MulAdd( closest2, -radiusB, normal ); + + int i1 = f1 == 0.0f ? 0 : 1; + int i2 = f2 == 0.0f ? 0 : 1; + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = sqrtf( distanceSquared ) - radius; + manifold.points[0].id = B2_MAKE_ID( i1, i2 ); + manifold.pointCount = 1; } // Convert manifold to world space @@ -528,9 +530,8 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const b2Manifold b2CollideSegmentAndCapsule( const b2Segment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ) { - b2Polygon polyA = b2MakeCapsule( segmentA->point1, segmentA->point2, 0.0f ); - b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius ); - return b2CollidePolygons( &polyA, xfA, &polyB, xfB ); + b2Capsule capsuleA = { segmentA->point1, segmentA->point2, 0.0f }; + return b2CollideCapsules( &capsuleA, xfA, capsuleB, xfB ); } b2Manifold b2CollidePolygonAndCapsule( const b2Polygon* polygonA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB )