Skip to content

Commit d077420

Browse files
erincattoerincatto
committed
capsule collider working better
fixed memory leak
1 parent a7b4ad4 commit d077420

File tree

4 files changed

+96
-89
lines changed

4 files changed

+96
-89
lines changed

‎samples/sample_collision.cpp‎

Lines changed: 12 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -2232,7 +2232,9 @@ class Manifold : public Sample
22322232
m_smgcapCache2 = b2_emptyDistanceCache;
22332233

22342234
m_transform = b2Transform_identity;
2235-
m_transform.p.x = 1.25f;
2235+
m_transform.p.x = 1.0f;
2236+
m_transform.p.y = 0.0f;
2237+
//m_transform.q = b2MakeRot( 0.5f * b2_pi );
22362238
m_angle = 0.0f;
22372239
m_round = 0.1f;
22382240

@@ -2382,7 +2384,6 @@ class Manifold : public Sample
23822384
m_smgcapCache2 = b2_emptyDistanceCache;
23832385
}
23842386

2385-
#if 0
23862387
// circle-circle
23872388
{
23882389
b2Circle circle1 = { { 0.0f, 0.0f }, 0.5f };
@@ -2461,31 +2462,30 @@ class Manifold : public Sample
24612462

24622463
offset = b2Add( offset, increment );
24632464
}
2464-
#endif
24652465

24662466
// capsule-capsule
24672467
{
2468-
b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f };
2468+
b2Capsule capsule1 = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f };
2469+
b2Capsule capsule2 = { { 0.25f, 0.0f }, { 1.0f, 0.0 }, 0.1f };
24692470

24702471
b2Transform transform1 = { offset, b2Rot_identity };
24712472
b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q };
24722473

2473-
b2Manifold m = b2CollideCapsules( &capsule, transform1, &capsule, transform2 );
2474+
b2Manifold m = b2CollideCapsules( &capsule1, transform1, &capsule2, transform2 );
24742475

2475-
b2Vec2 v1 = b2TransformPoint( transform1, capsule.center1 );
2476-
b2Vec2 v2 = b2TransformPoint( transform1, capsule.center2 );
2477-
g_draw.DrawSolidCapsule( v1, v2, capsule.radius, color1 );
2476+
b2Vec2 v1 = b2TransformPoint( transform1, capsule1.center1 );
2477+
b2Vec2 v2 = b2TransformPoint( transform1, capsule1.center2 );
2478+
g_draw.DrawSolidCapsule( v1, v2, capsule1.radius, color1 );
24782479

2479-
v1 = b2TransformPoint( transform2, capsule.center1 );
2480-
v2 = b2TransformPoint( transform2, capsule.center2 );
2481-
g_draw.DrawSolidCapsule( v1, v2, capsule.radius, color2 );
2480+
v1 = b2TransformPoint( transform2, capsule2.center1 );
2481+
v2 = b2TransformPoint( transform2, capsule2.center2 );
2482+
g_draw.DrawSolidCapsule( v1, v2, capsule2.radius, color2 );
24822483

24832484
DrawManifold( &m, transform1.p, transform2.p );
24842485

24852486
offset = b2Add( offset, increment );
24862487
}
24872488

2488-
#if 0
24892489
// box-capsule
24902490
{
24912491
b2Capsule capsule = { { -0.4f, 0.0f }, { -0.1f, 0.0f }, 0.1f };
@@ -2780,7 +2780,6 @@ class Manifold : public Sample
27802780

27812781
offset.x += 2.0f * increment.x;
27822782
}
2783-
#endif
27842783
}
27852784

27862785
static Sample* Create( Settings& settings )

‎src/array.h‎

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -46,10 +46,10 @@
4646
// Inline array functions that need the type T to be defined
4747
#define B2_ARRAY_INLINE( T, PREFIX ) \
4848
/* Resize */ \
49-
static inline void PREFIX##Array_Resize( PREFIX##Array* a, int count ) \
49+
static inline void PREFIX##Array_Resize( PREFIX##Array* a, int count ) \
5050
{ \
5151
PREFIX##Array_Reserve( a, count ); \
52-
a->count = count; \
52+
a->count = count; \
5353
} \
5454
/* Get */ \
5555
static inline T* PREFIX##Array_Get( PREFIX##Array* a, int index ) \
@@ -122,10 +122,12 @@
122122
/* Create */ \
123123
PREFIX##Array PREFIX##Array_Create( int capacity ) \
124124
{ \
125-
PREFIX##Array a; \
126-
a.data = b2Alloc( capacity * sizeof( T ) ); \
127-
a.count = 0; \
128-
a.capacity = capacity; \
125+
PREFIX##Array a = { 0 }; \
126+
if ( capacity > 0 ) \
127+
{ \
128+
a.data = b2Alloc( capacity * sizeof( T ) ); \
129+
a.capacity = capacity; \
130+
} \
129131
return a; \
130132
} \
131133
/* Reserve */ \

‎src/core.c‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,11 @@ void b2SetAllocator( b2AllocFcn* allocFcn, b2FreeFcn* freeFcn )
8282

8383
void* b2Alloc( int size )
8484
{
85+
if (size == 0)
86+
{
87+
return NULL;
88+
}
89+
8590
// This could cause some sharing issues, however Box2D rarely calls b2Alloc.
8691
atomic_fetch_add_explicit( &b2_byteCount, size, memory_order_relaxed );
8792

‎src/manifold.c‎

Lines changed: 71 additions & 70 deletions
Original file line numberDiff line numberDiff line change
@@ -318,57 +318,37 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const
318318
float distanceSquared = b2DistanceSquared( closest1, closest2 );
319319

320320
b2Manifold manifold = { 0 };
321-
float radius = capsuleA->radius + capsuleB->radius;
321+
float radiusA = capsuleA->radius;
322+
float radiusB = capsuleB->radius;
323+
float radius = radiusA + radiusB;
322324
float maxDistance = radius + b2_speculativeDistance;
323325

324326
if ( distanceSquared > maxDistance * maxDistance )
325327
{
326328
return manifold;
327329
}
328330

329-
b2Vec2 u1 = b2Normalize( d1 );
330-
b2Vec2 u2 = b2Normalize( d2 );
331+
float distance = sqrt( distanceSquared );
332+
333+
float length1, length2;
334+
b2Vec2 u1 = b2GetLengthAndNormalize( &length1, d1 );
335+
b2Vec2 u2 = b2GetLengthAndNormalize( &length2, d2 );
331336

332337
// Does segment B project outside segment A?
333338
float fp2 = b2Dot( b2Sub( p2, p1 ), u1 );
334339
float fq2 = b2Dot( b2Sub( q2, p1 ), u1 );
335-
bool outsideA = ( fp2 <= 0.0f && fq2 <= 0.0f ) || ( fp2 >= 1.0f && fq2 >= 1.0f );
340+
bool outsideA = ( fp2 <= 0.0f && fq2 <= 0.0f ) || ( fp2 >= length1 && fq2 >= length1 );
336341

337342
// Does segment A project outside segment B?
338343
float fp1 = b2Dot( b2Sub( p1, p2 ), u2 );
339344
float fq1 = b2Dot( b2Sub( q1, p2 ), u2 );
340-
bool outsideB = ( fp1 <= 0.0f && fq1 <= 0.0f ) || ( fp1 >= 1.0f && fq1 >= 1.0f );
341-
342-
bool flip = false;
343-
if ( outsideA && outsideB )
344-
{
345-
// vertex-vertex collision, no clipping needed
346-
347-
b2Vec2 normal = b2Sub( closest2, closest1 );
348-
if ( b2Dot( normal, normal ) > epsSqr )
349-
{
350-
normal = b2Normalize( normal );
351-
}
352-
else
353-
{
354-
normal = b2LeftPerp( u1 );
355-
}
356-
357-
b2Vec2 c1 = b2MulAdd( closest1, capsuleA->radius, normal );
358-
b2Vec2 c2 = b2MulAdd( closest1, -capsuleB->radius, normal );
345+
bool outsideB = ( fp1 <= 0.0f && fq1 <= 0.0f ) || ( fp1 >= length2 && fq1 >= length2 );
359346

360-
int i1 = f1 == 0.0f ? 0 : 1;
361-
int i2 = f2 == 0.0f ? 0 : 1;
362-
363-
manifold.normal = normal;
364-
manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f );
365-
manifold.points[0].separation = sqrtf(distanceSquared) - radius;
366-
manifold.points[0].id = B2_MAKE_ID( i1, i2 );
367-
manifold.pointCount = 1;
368-
}
369-
else
347+
if ( outsideA == false && outsideB == false)
370348
{
371-
// can clip
349+
// attempt to clip
350+
// this may yield contact points with excessive separation
351+
// in that case the algorithm falls back to single point collision
372352

373353
// find reference edge using SAT
374354
b2Vec2 normalA;
@@ -381,7 +361,7 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const
381361
float s1p = ss1 < ss2 ? ss1 : ss2;
382362
float s1n = -ss1 < -ss2 ? -ss1 : -ss2;
383363

384-
if (s1p > s1n)
364+
if ( s1p > s1n )
385365
{
386366
separationA = s1p;
387367
}
@@ -401,7 +381,7 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const
401381
float s1p = ss1 < ss2 ? ss1 : ss2;
402382
float s1n = -ss1 < -ss2 ? -ss1 : -ss2;
403383

404-
if (s1p > s1n)
384+
if ( s1p > s1n )
405385
{
406386
separationB = s1p;
407387
}
@@ -412,57 +392,55 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const
412392
}
413393
}
414394

415-
if (separationA > separationB)
395+
if ( separationA >= separationB )
416396
{
417397
manifold.normal = normalA;
418398

419399
b2Vec2 cp = p2;
420400
b2Vec2 cq = q2;
421401

422402
// clip to p1
423-
if (fp2 < 0.0f && fq2 > 0.0f)
403+
if ( fp2 < 0.0f && fq2 > 0.0f )
424404
{
425405
cp = b2Lerp( p2, q2, ( 0.0f - fp2 ) / ( fq2 - fp2 ) );
426406
}
427-
else if (fq2 < 0.0f && fp2 > 0.0f)
407+
else if ( fq2 < 0.0f && fp2 > 0.0f )
428408
{
429409
cq = b2Lerp( q2, p2, ( 0.0f - fq2 ) / ( fp2 - fq2 ) );
430410
}
431411

432412
// clip to q1
433-
if (fp2 > 1.0f && fq2 < 1.0f)
413+
if ( fp2 > length1 && fq2 < length1 )
434414
{
435-
cp = b2Lerp( q2, p2, ( fp2 - 1.0f ) / ( fp2 - fq2 ) );
415+
cp = b2Lerp( p2, q2, ( fp2 - length1 ) / ( fp2 - fq2 ) );
436416
}
437-
else if (fq2 > 1.0f && fp2 < 1.0f)
417+
else if ( fq2 > length1 && fp2 < length1 )
438418
{
439-
cq = b2Lerp( p2, q2, ( fq2 - 1.0f ) / ( fq2 - fp2 ) );
419+
cq = b2Lerp( q2, p2, ( fq2 - length1 ) / ( fq2 - fp2 ) );
440420
}
441421

442422
float sp = b2Dot( b2Sub( cp, p1 ), normalA );
443423
float sq = b2Dot( b2Sub( cq, p1 ), normalA );
444424

425+
if ( sp <= distance + b2_linearSlop || sq <= distance + b2_linearSlop )
445426
{
446-
b2ManifoldPoint* mp = manifold.points + 0;
447-
mp->anchorA = cp;
427+
b2ManifoldPoint* mp;
428+
mp = manifold.points + 0;
429+
mp->anchorA = b2MulAdd( cp, 0.5f * ( radiusA - radiusB - sp ), normalA );
448430
mp->separation = sp - radius;
449-
mp->id = B2_MAKE_ID(0, 0);
450-
}
431+
mp->id = B2_MAKE_ID( 0, 0 );
451432

452-
{
453-
b2ManifoldPoint* mp = manifold.points + 1;
454-
mp->anchorA = cq;
433+
mp = manifold.points + 1;
434+
mp->anchorA = b2MulAdd( cq, 0.5f * ( radiusA - radiusB - sq ), normalA );
455435
mp->separation = sq - radius;
456436
mp->id = B2_MAKE_ID( 0, 1 );
437+
manifold.pointCount = 2;
457438
}
458-
459-
manifold.pointCount = 2;
460439
}
461440
else
462441
{
463-
flip = true;
464-
465-
manifold.normal = normalB;
442+
// normal always points from A to B
443+
manifold.normal = b2Neg( normalB );
466444

467445
b2Vec2 cp = p1;
468446
b2Vec2 cq = q1;
@@ -478,34 +456,58 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const
478456
}
479457

480458
// clip to q2
481-
if ( fp1 > 1.0f && fq1 < 1.0f )
459+
if ( fp1 > length2 && fq1 < length2 )
482460
{
483-
cp = b2Lerp( q1, p1, ( fp1 - 1.0f ) / ( fp1 - fq1 ) );
461+
cp = b2Lerp( p1, q1, ( fp1 - length2 ) / ( fp1 - fq1 ) );
484462
}
485-
else if ( fq1 > 1.0f && fp1 < 1.0f )
463+
else if ( fq1 > length2 && fp1 < length2 )
486464
{
487-
cq = b2Lerp( p1, q1, ( fq1 - 1.0f ) / ( fq1 - fp1 ) );
465+
cq = b2Lerp( q1, p1, ( fq1 - length2 ) / ( fq1 - fp1 ) );
488466
}
489467

490468
float sp = b2Dot( b2Sub( cp, p2 ), normalB );
491469
float sq = b2Dot( b2Sub( cq, p2 ), normalB );
492470

471+
if ( sp <= distance + b2_linearSlop || sq <= distance + b2_linearSlop )
493472
{
494-
b2ManifoldPoint* mp = manifold.points + 0;
495-
mp->anchorA = cp;
473+
b2ManifoldPoint* mp;
474+
mp = manifold.points + 0;
475+
mp->anchorA = b2MulAdd( cp, 0.5f * ( radiusB - radiusA - sp ), normalB );
496476
mp->separation = sp - radius;
497477
mp->id = B2_MAKE_ID( 0, 0 );
498-
}
499-
500-
{
501-
b2ManifoldPoint* mp = manifold.points + 1;
502-
mp->anchorA = cq;
478+
mp = manifold.points + 1;
479+
mp->anchorA = b2MulAdd( cq, 0.5f * ( radiusB - radiusA - sq ), normalB );
503480
mp->separation = sq - radius;
504481
mp->id = B2_MAKE_ID( 1, 0 );
482+
manifold.pointCount = 2;
505483
}
484+
}
485+
}
506486

507-
manifold.pointCount = 2;
487+
if (manifold.pointCount == 0)
488+
{
489+
// single point collision
490+
b2Vec2 normal = b2Sub( closest2, closest1 );
491+
if ( b2Dot( normal, normal ) > epsSqr )
492+
{
493+
normal = b2Normalize( normal );
508494
}
495+
else
496+
{
497+
normal = b2LeftPerp( u1 );
498+
}
499+
500+
b2Vec2 c1 = b2MulAdd( closest1, radiusA, normal );
501+
b2Vec2 c2 = b2MulAdd( closest2, -radiusB, normal );
502+
503+
int i1 = f1 == 0.0f ? 0 : 1;
504+
int i2 = f2 == 0.0f ? 0 : 1;
505+
506+
manifold.normal = normal;
507+
manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f );
508+
manifold.points[0].separation = sqrtf( distanceSquared ) - radius;
509+
manifold.points[0].id = B2_MAKE_ID( i1, i2 );
510+
manifold.pointCount = 1;
509511
}
510512

511513
// Convert manifold to world space
@@ -528,9 +530,8 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const
528530

529531
b2Manifold b2CollideSegmentAndCapsule( const b2Segment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB )
530532
{
531-
b2Polygon polyA = b2MakeCapsule( segmentA->point1, segmentA->point2, 0.0f );
532-
b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius );
533-
return b2CollidePolygons( &polyA, xfA, &polyB, xfB );
533+
b2Capsule capsuleA = { segmentA->point1, segmentA->point2, 0.0f };
534+
return b2CollideCapsules( &capsuleA, xfA, capsuleB, xfB );
534535
}
535536

536537
b2Manifold b2CollidePolygonAndCapsule( const b2Polygon* polygonA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB )

0 commit comments

Comments
 (0)