From df7373c08a41b7a4ba6edd5d4be200675a948176 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Wed, 25 Sep 2024 22:43:34 -0700 Subject: [PATCH] New capsule collider (#804) Wrote a custom capsule versus capsule collider which is more robust than treating capsules as two face polygons. --- include/box2d/box2d.h | 16 ++ include/box2d/collision.h | 10 ++ samples/sample_bodies.cpp | 2 +- samples/sample_collision.cpp | 22 +-- samples/sample_continuous.cpp | 56 +++++++ src/array.h | 16 +- src/body.c | 10 ++ src/broad_phase.c | 2 + src/core.c | 5 + src/distance.c | 2 +- src/distance_joint.c | 2 + src/joint.c | 6 + src/manifold.c | 286 ++++++++++++++++++++++++++++++++-- src/revolute_joint.c | 2 + src/shape.c | 35 +++++ src/solver.c | 19 ++- src/world.c | 2 + 17 files changed, 460 insertions(+), 33 deletions(-) diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 99ddb1d73..8d7634be1 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -467,6 +467,9 @@ B2_API b2ShapeType b2Shape_GetType( b2ShapeId shapeId ); /// Get the id of the body that a shape is attached to B2_API b2BodyId b2Shape_GetBody( b2ShapeId shapeId ); +/// Get the world that owns this shape +B2_API b2WorldId b2Shape_GetWorld( b2ShapeId shapeId ); + /// Returns true If the shape is a sensor B2_API bool b2Shape_IsSensor( b2ShapeId shapeId ); @@ -600,6 +603,16 @@ B2_API b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ); /// Destroy a chain shape B2_API void b2DestroyChain( b2ChainId chainId ); +/// Get the world that owns this chain shape +B2_API b2WorldId b2Chain_GetWorld( b2ChainId chainId ); + +/// Get the number of segments on this chain +B2_API int b2Chain_GetSegmentCount( b2ChainId chainId ); + +/// Fill a user array with chain segment shape ids up to the specified capacity. Returns +/// the actual number of segments returned. +B2_API int b2Chain_GetSegments( b2ChainId chainId, b2ShapeId* segmentArray, int capacity ); + /// Set the chain friction /// @see b2ChainDef::friction B2_API void b2Chain_SetFriction( b2ChainId chainId, float friction ); @@ -634,6 +647,9 @@ B2_API b2BodyId b2Joint_GetBodyA( b2JointId jointId ); /// Get body B id on a joint B2_API b2BodyId b2Joint_GetBodyB( b2JointId jointId ); +/// Get the world that owns this joint +B2_API b2WorldId b2Joint_GetWorld( b2JointId jointId ); + /// Get the local anchor on bodyA B2_API b2Vec2 b2Joint_GetLocalAnchorA( b2JointId jointId ); diff --git a/include/box2d/collision.h b/include/box2d/collision.h index facd1fef2..4b296c0b3 100644 --- a/include/box2d/collision.h +++ b/include/box2d/collision.h @@ -183,15 +183,25 @@ B2_API b2Polygon b2MakePolygon( const b2Hull* hull, float radius ); B2_API b2Polygon b2MakeOffsetPolygon( const b2Hull* hull, float radius, b2Transform transform ); /// Make a square polygon, bypassing the need for a convex hull. +/// @param h the half-width B2_API b2Polygon b2MakeSquare( float h ); /// Make a box (rectangle) polygon, bypassing the need for a convex hull. +/// @param hx the half-width +/// @param hy the half-height B2_API b2Polygon b2MakeBox( float hx, float hy ); /// Make a rounded box, bypassing the need for a convex hull. +/// @param hx the half-width +/// @param hy the half-height +/// @param radius the radius of the rounded extension B2_API b2Polygon b2MakeRoundedBox( float hx, float hy, float radius ); /// Make an offset box, bypassing the need for a convex hull. +/// @param hx the half-width +/// @param hy the half-height +/// @param center the local position of the center of the box +/// @param rotation the local rotation of the box B2_API b2Polygon b2MakeOffsetBox( float hx, float hy, b2Vec2 center, b2Rot rotation ); /// Transform a polygon. This is useful for transferring a shape from one body to another. diff --git a/samples/sample_bodies.cpp b/samples/sample_bodies.cpp index 1d5017cd8..cc441a505 100644 --- a/samples/sample_bodies.cpp +++ b/samples/sample_bodies.cpp @@ -785,7 +785,7 @@ class BadBody : public Sample b2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = b2_dynamicBody; bodyDef.position = { 0.0f, 3.0f }; - bodyDef.angularVelocity = 0.2f; + bodyDef.angularVelocity = 0.5f; bodyDef.rotation = b2MakeRot( 0.25f * b2_pi ); m_badBodyId = b2CreateBody( m_worldId, &bodyDef ); diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index 038a3ad5b..9241152e4 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -2232,6 +2232,9 @@ class Manifold : public Sample m_smgcapCache2 = b2_emptyDistanceCache; m_transform = b2Transform_identity; + 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; @@ -2371,7 +2374,7 @@ class Manifold : public Sample b2Vec2 increment = { 4.0f, 0.0f }; b2HexColor color1 = b2_colorAquamarine; - b2HexColor color2 = b2_colorMagenta; + b2HexColor color2 = b2_colorPaleGoldenrod; if ( m_enableCaching == false ) { @@ -2462,20 +2465,21 @@ class Manifold : public Sample // 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 ); diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index a8dbe85fa..ac35e07c8 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -389,6 +389,62 @@ class SkinnyBox : public Sample static int sampleSkinnyBox = RegisterSample( "Continuous", "Skinny Box", SkinnyBox::Create ); +class SpeculativeBug : public Sample +{ +public: + explicit SpeculativeBug( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 1.0f, 5.0f }; + g_camera.m_zoom = 25.0f * 0.25f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Segment segment = { { -10.0f, 0.0f }, { 10.0f, 0.0f } }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + shapeDef.friction = 0.0f; + b2Polygon box = b2MakeOffsetBox( 0.05f, 1.0f, { 0.0f, 1.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + for (int i = 0; i < 2; ++i) + { + if (i == 0) + { + bodyDef.position = { -0.8f, 0.25f }; + bodyDef.isAwake = false; + } + else + { + bodyDef.position = { 0.8f, 2.0f }; + bodyDef.isAwake = true; + } + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0f }, 0.25f }; + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + } + + static Sample* Create( Settings& settings ) + { + return new SpeculativeBug( settings ); + } +}; + +static int sampleSpeculativeBug = RegisterSample( "Continuous", "Speculative Bug", SpeculativeBug::Create ); + // This sample shows ghost collisions class GhostCollision : public Sample { diff --git a/src/array.h b/src/array.h index ea035f679..e590d292f 100644 --- a/src/array.h +++ b/src/array.h @@ -5,7 +5,7 @@ #include "core.h" -// Macro based dynamic arrays +// Macro generated functions for dynamic arrays // Pros // - type safe // - array data debuggable (visible count and capacity) @@ -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/body.c b/src/body.c index 4d0ed82fa..639914258 100644 --- a/src/body.c +++ b/src/body.c @@ -744,6 +744,11 @@ void b2Body_SetLinearVelocity( b2BodyId bodyId, b2Vec2 linearVelocity ) b2World* world = b2GetWorld( bodyId.world0 ); b2Body* body = b2GetBodyFullId( world, bodyId ); + if ( body->type == b2_staticBody ) + { + return; + } + if ( b2LengthSquared( linearVelocity ) > 0.0f ) { b2WakeBody( world, body ); @@ -763,6 +768,11 @@ void b2Body_SetAngularVelocity( b2BodyId bodyId, float angularVelocity ) b2World* world = b2GetWorld( bodyId.world0 ); b2Body* body = b2GetBodyFullId( world, bodyId ); + if (body->type == b2_staticBody || body->fixedRotation) + { + return; + } + if ( angularVelocity != 0.0f ) { b2WakeBody( world, body ); diff --git a/src/broad_phase.c b/src/broad_phase.c index 0f26523ff..8043ea13d 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -1,7 +1,9 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) #define _CRT_SECURE_NO_WARNINGS +#endif #include "broad_phase.h" 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/distance.c b/src/distance.c index 048860c0a..25f2abc2c 100644 --- a/src/distance.c +++ b/src/distance.c @@ -38,8 +38,8 @@ b2SegmentDistanceResult b2SegmentDistance( b2Vec2 p1, b2Vec2 q1, b2Vec2 p2, b2Ve b2Vec2 r = b2Sub( p1, p2 ); float dd1 = b2Dot( d1, d1 ); float dd2 = b2Dot( d2, d2 ); - float rd2 = b2Dot( r, d2 ); float rd1 = b2Dot( r, d1 ); + float rd2 = b2Dot( r, d2 ); const float epsSqr = FLT_EPSILON * FLT_EPSILON; diff --git a/src/distance_joint.c b/src/distance_joint.c index 5763c06ca..cec457395 100644 --- a/src/distance_joint.c +++ b/src/distance_joint.c @@ -1,7 +1,9 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) #define _CRT_SECURE_NO_WARNINGS +#endif #include "body.h" #include "core.h" diff --git a/src/joint.c b/src/joint.c index c72f7734b..d1af86f83 100644 --- a/src/joint.c +++ b/src/joint.c @@ -789,6 +789,12 @@ b2BodyId b2Joint_GetBodyB( b2JointId jointId ) return b2MakeBodyId( world, joint->edges[1].bodyId ); } +b2WorldId b2Joint_GetWorld( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + return ( b2WorldId ){ jointId.world0 + 1, world->revision }; +} + b2Vec2 b2Joint_GetLocalAnchorA( b2JointId jointId ) { b2World* world = b2GetWorld( jointId.world0 ); diff --git a/src/manifold.c b/src/manifold.c index 74b94c5be..b66785d28 100644 --- a/src/manifold.c +++ b/src/manifold.c @@ -20,7 +20,7 @@ static b2Polygon b2MakeCapsule( b2Vec2 p1, b2Vec2 p2, float radius ) b2Vec2 d = b2Sub( p2, p1 ); B2_ASSERT( b2LengthSquared( d ) > FLT_EPSILON ); - b2Vec2 axis = b2Normalize(d); + b2Vec2 axis = b2Normalize( d ); b2Vec2 normal = b2RightPerp( axis ); shape.normals[0] = normal; @@ -254,18 +254,284 @@ b2Manifold b2CollidePolygonAndCircle( const b2Polygon* polygonA, b2Transform xfA return manifold; } +// Follows Ericson 5.1.9 Closest Points of Two Line Segments +// Adds some logic to support clipping to get two contact points b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ) { - b2Polygon polyA = b2MakeCapsule( capsuleA->center1, capsuleA->center2, capsuleA->radius ); - b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius ); - return b2CollidePolygons( &polyA, xfA, &polyB, xfB ); + b2Vec2 origin = capsuleA->center1; + + // Shift polyA to origin + // pw = q * pb + p + // pw = q * (pbs + origin) + p + // pw = q * pbs + (p + q * origin) + b2Transform sfA = { b2Add( xfA.p, b2RotateVector( xfA.q, origin ) ), xfA.q }; + b2Transform xf = b2InvMulTransforms( sfA, xfB ); + + b2Vec2 p1 = b2Vec2_zero; + b2Vec2 q1 = b2Sub( capsuleA->center2, origin ); + + b2Vec2 p2 = b2TransformPoint( xf, capsuleB->center1 ); + b2Vec2 q2 = b2TransformPoint( xf, capsuleB->center2 ); + + b2Vec2 d1 = b2Sub( q1, p1 ); + b2Vec2 d2 = b2Sub( q2, p2 ); + + float dd1 = b2Dot( d1, d1 ); + float dd2 = b2Dot( d2, d2 ); + + const float epsSqr = FLT_EPSILON * FLT_EPSILON; + B2_ASSERT( dd1 > epsSqr && dd2 > epsSqr ); + + b2Vec2 r = b2Sub( p1, p2 ); + float rd1 = b2Dot( r, d1 ); + float rd2 = b2Dot( r, d2 ); + + float d12 = b2Dot( d1, d2 ); + + float denom = dd1 * dd2 - d12 * d12; + + // Fraction on segment 1 + float f1 = 0.0f; + if ( denom != 0.0f ) + { + // not parallel + f1 = b2ClampFloat( ( d12 * rd2 - rd1 * dd2 ) / denom, 0.0f, 1.0f ); + } + + // Compute point on segment 2 closest to p1 + f1 * d1 + float f2 = ( d12 * f1 + rd2 ) / dd2; + + // Clamping of segment 2 requires a do over on segment 1 + if ( f2 < 0.0f ) + { + f2 = 0.0f; + f1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f ); + } + else if ( f2 > 1.0f ) + { + f2 = 1.0f; + f1 = b2ClampFloat( ( d12 - rd1 ) / dd1, 0.0f, 1.0f ); + } + + b2Vec2 closest1 = b2MulAdd( p1, f1, d1 ); + b2Vec2 closest2 = b2MulAdd( p2, f2, d2 ); + float distanceSquared = b2DistanceSquared( closest1, closest2 ); + + b2Manifold manifold = { 0 }; + float radiusA = capsuleA->radius; + float radiusB = capsuleB->radius; + float radius = radiusA + radiusB; + float maxDistance = radius + b2_speculativeDistance; + + if ( distanceSquared > maxDistance * maxDistance ) + { + return manifold; + } + + 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 >= 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 >= length2 && fq1 >= length2 ); + + if ( outsideA == false && outsideB == false) + { + // 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; + float separationA; + + { + normalA = b2LeftPerp( u1 ); + float ss1 = b2Dot( b2Sub( p2, p1 ), normalA ); + float ss2 = b2Dot( b2Sub( q2, p1 ), normalA ); + float s1p = ss1 < ss2 ? ss1 : ss2; + float s1n = -ss1 < -ss2 ? -ss1 : -ss2; + + if ( s1p > s1n ) + { + separationA = s1p; + } + else + { + separationA = s1n; + normalA = b2Neg( normalA ); + } + } + + b2Vec2 normalB; + float separationB; + { + normalB = b2LeftPerp( u2 ); + float ss1 = b2Dot( b2Sub( p1, p2 ), normalB ); + float ss2 = b2Dot( b2Sub( q1, p2 ), normalB ); + float s1p = ss1 < ss2 ? ss1 : ss2; + float s1n = -ss1 < -ss2 ? -ss1 : -ss2; + + if ( s1p > s1n ) + { + separationB = s1p; + } + else + { + separationB = s1n; + normalB = b2Neg( normalB ); + } + } + + if ( separationA >= separationB ) + { + manifold.normal = normalA; + + b2Vec2 cp = p2; + b2Vec2 cq = q2; + + // clip to p1 + if ( fp2 < 0.0f && fq2 > 0.0f ) + { + cp = b2Lerp( p2, q2, ( 0.0f - fp2 ) / ( fq2 - fp2 ) ); + } + else if ( fq2 < 0.0f && fp2 > 0.0f ) + { + cq = b2Lerp( q2, p2, ( 0.0f - fq2 ) / ( fp2 - fq2 ) ); + } + + // clip to q1 + if ( fp2 > length1 && fq2 < length1 ) + { + cp = b2Lerp( p2, q2, ( fp2 - length1 ) / ( fp2 - fq2 ) ); + } + else if ( fq2 > length1 && fp2 < length1 ) + { + 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; + 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 = 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; + } + } + else + { + // normal always points from A to B + manifold.normal = b2Neg( normalB ); + + b2Vec2 cp = p1; + b2Vec2 cq = q1; + + // clip to p2 + if ( fp1 < 0.0f && fq1 > 0.0f ) + { + cp = b2Lerp( p1, q1, ( 0.0f - fp1 ) / ( fq1 - fp1 ) ); + } + else if ( fq1 < 0.0f && fp1 > 0.0f ) + { + cq = b2Lerp( q1, p1, ( 0.0f - fq1 ) / ( fp1 - fq1 ) ); + } + + // clip to q2 + if ( fp1 > length2 && fq1 < length2 ) + { + cp = b2Lerp( p1, q1, ( fp1 - length2 ) / ( fp1 - fq1 ) ); + } + else if ( fq1 > length2 && fp1 < length2 ) + { + 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; + 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 ); + 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; + } + } + } + + 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 + if ( manifold.pointCount > 0 ) + { + manifold.normal = b2RotateVector( xfA.q, manifold.normal ); + for ( int i = 0; i < manifold.pointCount; ++i ) + { + b2ManifoldPoint* mp = manifold.points + i; + + // anchor points relative to shape origin in world space + mp->anchorA = b2RotateVector( xfA.q, b2Add( mp->anchorA, origin ) ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + } + } + + return manifold; } 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 ) @@ -714,7 +980,7 @@ b2Manifold b2CollideSegmentAndPolygon( const b2Segment* segmentA, b2Transform xf } b2Manifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, b2Transform xfA, const b2Circle* circleB, - b2Transform xfB ) + b2Transform xfB ) { b2Manifold manifold = { 0 }; @@ -802,7 +1068,7 @@ b2Manifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, b2Tra } b2Manifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, - b2Transform xfB, b2DistanceCache* cache ) + b2Transform xfB, b2DistanceCache* cache ) { b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius ); return b2CollideChainSegmentAndPolygon( segmentA, xfA, &polyB, xfB, cache ); @@ -944,7 +1210,7 @@ static enum b2NormalType b2ClassifyNormal( struct b2ChainSegmentParams params, b } b2Manifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA, b2Transform xfA, const b2Polygon* polygonB, - b2Transform xfB, b2DistanceCache* cache ) + b2Transform xfB, b2DistanceCache* cache ) { b2Manifold manifold = { 0 }; diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 959ca3ace..98b8eb09c 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -1,7 +1,9 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) #define _CRT_SECURE_NO_WARNINGS +#endif #include "body.h" #include "core.h" diff --git a/src/shape.c b/src/shape.c index 17880f1ba..9d4cb8bac 100644 --- a/src/shape.c +++ b/src/shape.c @@ -440,6 +440,35 @@ void b2DestroyChain( b2ChainId chainId ) b2ValidateSolverSets( world ); } +b2WorldId b2Chain_GetWorld(b2ChainId chainId) +{ + b2World* world = b2GetWorld( chainId.world0 ); + return ( b2WorldId ){ chainId.world0 + 1, world->revision }; +} + +int b2Chain_GetSegmentCount(b2ChainId chainId) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + b2ChainShape* chain = b2GetChainShape( world, chainId ); + return chain->count; +} + +int b2Chain_GetSegments(b2ChainId chainId, b2ShapeId* segmentArray, int capacity) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + b2ChainShape* chain = b2GetChainShape( world, chainId ); + + int count = b2MinInt(chain->count, capacity); + for ( int i = 0; i < count; ++i ) + { + int shapeId = chain->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + segmentArray[i] = ( b2ShapeId ){ shapeId + 1, chainId.world0, shape->revision }; + } + + return count; +} + b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2Transform xf ) { switch ( shape->type ) @@ -726,6 +755,12 @@ b2BodyId b2Shape_GetBody( b2ShapeId shapeId ) return b2MakeBodyId( world, shape->bodyId ); } +b2WorldId b2Shape_GetWorld(b2ShapeId shapeId) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + return ( b2WorldId ){ shapeId.world0 + 1, world->revision }; +} + void b2Shape_SetUserData( b2ShapeId shapeId, void* userData ) { b2World* world = b2GetWorld( shapeId.world0 ); diff --git a/src/solver.c b/src/solver.c index 1166a1e25..28a4c2b25 100644 --- a/src/solver.c +++ b/src/solver.c @@ -22,21 +22,30 @@ #include #include -#if defined( B2_CPU_ARM ) +#if ( defined( __GNUC__ ) || defined( __clang__ ) ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) static inline void b2Pause( void ) { - __asm__ __volatile__( "isb\n" ); + __asm__ __volatile__( "pause\n" ); } -#elif defined( B2_CPU_X86_X64 ) -#include +#elif ( defined( __arm__ ) && defined( __ARM_ARCH ) && __ARM_ARCH >= 7 ) || defined( __aarch64__ ) +static inline void b2Pause( void ) +{ + __asm__ __volatile__( "yield" ::: "memory" ); +} +#elif defined( _MSC_VER ) && ( defined( _M_IX86 ) || defined( _M_X64 ) ) +//#include static inline void b2Pause( void ) { _mm_pause(); } +#elif defined( _MSC_VER ) && ( defined( _M_ARM ) || defined( _M_ARM64 ) ) +static inline void b2Pause( void ) +{ + __yield(); +} #else static inline void b2Pause( void ) { - // no threading will likely be used in web assembly } #endif diff --git a/src/world.c b/src/world.c index 86e344164..53d462dac 100644 --- a/src/world.c +++ b/src/world.c @@ -1,7 +1,9 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) #define _CRT_SECURE_NO_WARNINGS +#endif #include "world.h"