Skip to content

Commit

Permalink
capsule collider working better
Browse files Browse the repository at this point in the history 
fixed memory leak
  • Loading branch information
@erincatto
erincatto committed Sep 24, 2024
1 parent a7b4ad4 commit d077420
Show file tree
Hide file tree
Showing 4 changed files with 96 additions and 89 deletions.
25 changes: 12 additions & 13 deletions samples/sample_collision.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -2382,7 +2384,6 @@ class Manifold : public Sample
m_smgcapCache2 = b2_emptyDistanceCache;
}

#if 0
// circle-circle
{
b2Circle circle1 = { { 0.0f, 0.0f }, 0.5f };
Expand Down Expand Up @@ -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 };
Expand Down Expand Up @@ -2780,7 +2780,6 @@ class Manifold : public Sample

offset.x += 2.0f * increment.x;
}
#endif
}

static Sample* Create( Settings& settings )
Expand Down
14 changes: 8 additions & 6 deletions src/array.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 ) \
Expand Down Expand Up @@ -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 */ \
Expand Down
5 changes: 5 additions & 0 deletions src/core.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 );

Expand Down
141 changes: 71 additions & 70 deletions src/manifold.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -318,57 +318,37 @@ 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 )
{
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;
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -412,57 +392,55 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const
}
}

if (separationA > separationB)
if ( separationA >= separationB )
{
manifold.normal = normalA;

b2Vec2 cp = p2;
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;
Expand All @@ -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
Expand All @@ -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 )
Expand Down

0 comments on commit d077420

Please sign in to comment.