Compare commits

...
Sign in to create a new pull request.
Author SHA1 Message Date
Erin Catto
d683c8f0cc update readme about 32 bit support 2026-11-17 23:34:23 -08:00
Erin Catto
f36bcdaccc simplify manifold allocation 2026-11-17 23:01:38 -08:00
Erin Catto
6357bf521a stop overriding mass when setting motion locks 2026-11-17 22:50:41 -08:00
Erin Catto
6de662cd3b Fix for sbox fastmath usage 2026-11-17 20:49:37 -08:00
14 changed files with 275 additions and 178 deletions

View file

@ -143,6 +143,8 @@ See [`docs/hello.md`](docs/hello.md) for a minimal first program.
The Box3D library and samples build and run on Windows, Linux, and Mac. The Box3D library and samples build and run on Windows, Linux, and Mac.
Box3D only supports 64-bit desktop platforms. 32-bit builds, such as Win32, are not supported.
You will need a compiler that supports C17 to build the Box3D library. You will need a compiler that supports C17 to build the Box3D library.
You will need a compiler that supports C++20 to build the samples. You will need a compiler that supports C++20 to build the samples.

View file

@ -315,7 +315,9 @@ typedef enum b3BodyType
b3_bodyTypeCount, b3_bodyTypeCount,
} b3BodyType; } b3BodyType;
/// Motion locks to restrict the body movement /// Motion locks to restrict the body movement. Motion locks are a hack and will likely be removed. The only robust
/// lock is fully locked rotation. You will see softened contacts and other artifacts with this sytem. Other physics
/// engines have the same problem and some have stability problems due to this approach.
/// @ingroup body /// @ingroup body
typedef struct b3MotionLocks typedef struct b3MotionLocks
{ {

View file

@ -1211,7 +1211,7 @@ public:
b3BodyDef bodyDef = b3DefaultBodyDef(); b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody; bodyDef.type = b3_dynamicBody;
bodyDef.name = "full"; bodyDef.name = "full";
bodyDef.position = { 0.0f, 1.0f, 2.0f }; bodyDef.position = { 0.0f, 1.0f, 3.0f };
bodyDef.motionLocks.linearX = true; bodyDef.motionLocks.linearX = true;
bodyDef.motionLocks.linearY = true; bodyDef.motionLocks.linearY = true;
bodyDef.motionLocks.linearZ = true; bodyDef.motionLocks.linearZ = true;

View file

@ -3093,3 +3093,73 @@ public:
}; };
static int sampleStaticLoad = RegisterSample( "Issues", "Static Load", StaticLoad::Create ); static int sampleStaticLoad = RegisterSample( "Issues", "Static Load", StaticLoad::Create );
class TranslationLockedPlatform : public Sample
{
public:
explicit TranslationLockedPlatform( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 10.0f, 10.0f, { 0.0f, 0.5f, 0.0f } );
}
context->hertz = 50.0f;
context->subStepCount = 4;
b3World_SetGravity( m_worldId, { 0.0f, -20.0f, 0.0f } );
for ( int i = 0; i < 2; ++i )
{
float x = i == 0 ? -3.0f : 3.0f;
b3BodyDef groundDef = b3DefaultBodyDef();
groundDef.type = i == 0 ? b3_staticBody : b3_dynamicBody;
groundDef.position = { x, 0.0f, 0.0f };
groundDef.enableSleep = false;
groundDef.motionLocks.linearX = true;
groundDef.motionLocks.linearY = true;
groundDef.motionLocks.linearZ = true;
groundDef.motionLocks.angularX = true;
groundDef.motionLocks.angularY = true;
b3BodyId groundId = b3CreateBody( m_worldId, &groundDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.density = 2.5f;
b3BoxHull box = b3MakeBoxHull( 2.0f, 0.05f, 0.5f );
b3CreateHullShape( groundId, &shapeDef, &box.base );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = { x, 0.65f, 0.0f };
bodyDef.enableSleep = false;
m_spheres[i] = b3CreateBody( m_worldId, &bodyDef );
b3Sphere sphere = { b3Vec3_zero, 0.5f };
b3CreateSphereShape( m_spheres[i], &shapeDef, &sphere );
b3MassData massData = b3Body_GetMassData( m_spheres[i] );
massData.inertia = b3MulSM( 10.0f / massData.mass, massData.inertia );
massData.mass = 10.0f;
b3Body_SetMassData( m_spheres[i], massData );
}
}
void Render() override
{
DrawTextLine( "10 kg spheres dropped onto 0.1 m thick platforms." );
DrawTextLine( "Right platform: 1 kg, translation locked, rotation about Z free." );
DrawTextLine( "Expected: both gaps settle near zero. Negative means penetration." );
DrawTextLine( "Left (static): gap = %.3f m", (float)b3Body_GetPosition( m_spheres[0] ).y - 0.55f );
DrawTextLine( "Right (locked): gap = %.3f m", (float)b3Body_GetPosition( m_spheres[1] ).y - 0.55f );
}
static Sample* Create( SampleContext* context )
{
return new TranslationLockedPlatform( context );
}
b3BodyId m_spheres[2];
};
static int sampleTranslationLockedPlatform =
RegisterSample( "Issues", "Translation Locked Platform", TranslationLockedPlatform::Create );

View file

@ -2035,6 +2035,9 @@ public:
} }
m_motionLocks = {}; m_motionLocks = {};
m_motionLocks.linearX = true;
m_motionLocks.linearY = true;
m_motionLocks.linearZ = true;
for ( int i = 0; i < m_capacity; ++i ) for ( int i = 0; i < m_capacity; ++i )
{ {
@ -2045,6 +2048,7 @@ public:
b3BodyDef bodyDef = b3DefaultBodyDef(); b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody; bodyDef.type = b3_dynamicBody;
bodyDef.enableSleep = false; bodyDef.enableSleep = false;
bodyDef.motionLocks = m_motionLocks;
b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 0.5f ); b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 0.5f );
@ -2056,6 +2060,7 @@ public:
b3ShapeDef shapeDef = b3DefaultShapeDef(); b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.density = 1.0f; shapeDef.density = 1.0f;
#if 1
// distance joint // distance joint
{ {
assert( m_count < m_capacity ); assert( m_count < m_capacity );
@ -2086,30 +2091,28 @@ public:
++m_count; ++m_count;
// motor joint // motor joint
#if 0
{ {
assert( m_bodyCount < e_count ); assert( m_count < m_capacity );
bodyDef.position = position; bodyDef.position = position;
m_bodyIds[m_bodyCount] = b3CreateBody( m_worldId, &bodyDef ); m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( m_bodyIds[m_bodyCount], &shapeDef, &box.base ); b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
b3MotorJointDef jointDef = b3DefaultMotorJointDef(); b3MotorJointDef jointDef = b3DefaultMotorJointDef();
jointDef.base.bodyIdA = groundId; jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = m_bodyIds[m_bodyCount]; jointDef.base.bodyIdB = m_bodyIds[m_count];
jointDef.base.localFrameA.p = position; jointDef.base.localFrameA.p = position;
jointDef.maxForce = 1000.0f; jointDef.maxSpringForce = 1000.0f;
jointDef.maxTorque = 20.0f; jointDef.maxSpringTorque = 20.0f;
jointDef.base.forceThreshold = forceThreshold; jointDef.base.forceThreshold = forceThreshold;
jointDef.base.torqueThreshold = torqueThreshold; jointDef.base.torqueThreshold = torqueThreshold;
jointDef.base.collideConnected = true; jointDef.base.collideConnected = true;
jointDef.base.userData = (void*)(intptr_t)m_bodyCount; jointDef.base.userData = (void*)(intptr_t)m_count;
b3CreateMotorJoint( m_worldId, &jointDef ); b3CreateMotorJoint( m_worldId, &jointDef );
} }
position.x += 5.0f; position.x += 5.0f;
++m_count; ++m_count;
#endif
// prismatic joint // prismatic joint
{ {
@ -2131,6 +2134,7 @@ public:
jointDef.base.userData = (void*)(intptr_t)m_count; jointDef.base.userData = (void*)(intptr_t)m_count;
b3CreatePrismaticJoint( m_worldId, &jointDef ); b3CreatePrismaticJoint( m_worldId, &jointDef );
} }
#endif
position.x += 5.0f; position.x += 5.0f;
++m_count; ++m_count;
@ -2159,6 +2163,7 @@ public:
position.x += 5.0f; position.x += 5.0f;
++m_count; ++m_count;
#if 1
// weld joint // weld joint
{ {
assert( m_count < m_capacity ); assert( m_count < m_capacity );
@ -2186,9 +2191,8 @@ public:
++m_count; ++m_count;
// wheel joint // wheel joint
#if 0
{ {
assert( index < e_count ); assert( m_count < m_capacity );
bodyDef.position = position; bodyDef.position = position;
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
@ -2200,18 +2204,18 @@ public:
jointDef.base.bodyIdB = bodyId; jointDef.base.bodyIdB = bodyId;
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot ); jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot ); jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot );
jointDef.hertz = 1.0f; jointDef.suspensionHertz = 1.0f;
jointDef.dampingRatio = 0.7f; jointDef.suspensionDampingRatio = 0.7f;
jointDef.lowerTranslation = -1.0f; jointDef.lowerSuspensionLimit = -1.0f;
jointDef.upperTranslation = 1.0f; jointDef.upperSuspensionLimit = 1.0f;
jointDef.enableLimit = true; jointDef.enableSuspensionLimit = true;
jointDef.enableMotor = true; jointDef.enableSpinMotor = true;
jointDef.maxMotorTorque = 10.0f; jointDef.maxSpinTorque = 10.0f;
jointDef.motorSpeed = 1.0f; jointDef.spinSpeed = 1.0f;
jointDef.base.forceThreshold = forceThreshold; jointDef.base.forceThreshold = forceThreshold;
jointDef.base.torqueThreshold = torqueThreshold; jointDef.base.torqueThreshold = torqueThreshold;
jointDef.base.collideConnected = true; jointDef.base.collideConnected = true;
jointDef.base.userData = (void*)(intptr_t)index; jointDef.base.userData = (void*)(intptr_t)m_count;
b3CreateWheelJoint( m_worldId, &jointDef ); b3CreateWheelJoint( m_worldId, &jointDef );
} }
@ -2289,7 +2293,7 @@ public:
return new MotionLocks( context ); return new MotionLocks( context );
} }
static constexpr int m_capacity = 6; static constexpr int m_capacity = 7;
b3BodyId m_bodyIds[m_capacity]; b3BodyId m_bodyIds[m_capacity];
int m_count; int m_count;
b3MotionLocks m_motionLocks; b3MotionLocks m_motionLocks;

View file

@ -500,7 +500,8 @@ int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, int capa
b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA );
b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB );
contactData[index].contactId = (b3ContactId){ contact->contactId + 1, bodyId.world0, 0, contact->generation }; contactData[index].contactId =
(b3ContactId){ contact->contactId + 1, bodyId.world0, 0, contact->generation };
contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation }; contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation };
contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, bodyId.world0, shapeB->generation }; contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, bodyId.world0, shapeB->generation };
contactData[index].manifolds = contact->manifolds; contactData[index].manifolds = contact->manifolds;
@ -593,8 +594,8 @@ float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target )
return closestDistance; return closestDistance;
} }
b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, float maxFraction, b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter,
b3WorldTransform bodyTransform ) float maxFraction, b3WorldTransform bodyTransform )
{ {
b3World* world = b3GetUnlockedWorld( bodyId.world0 ); b3World* world = b3GetUnlockedWorld( bodyId.world0 );
if ( world == NULL ) if ( world == NULL )
@ -660,7 +661,8 @@ b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translati
} }
b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,
b3QueryFilter filter, float maxFraction, bool canEncroach, b3WorldTransform bodyTransform ) b3QueryFilter filter, float maxFraction, bool canEncroach,
b3WorldTransform bodyTransform )
{ {
b3World* world = b3GetUnlockedWorld( bodyId.world0 ); b3World* world = b3GetUnlockedWorld( bodyId.world0 );
if ( world == NULL ) if ( world == NULL )
@ -757,8 +759,8 @@ bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* pro
return false; return false;
} }
int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, const b3Capsule* mover, int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin,
b3QueryFilter filter, b3WorldTransform bodyTransform ) const b3Capsule* mover, b3QueryFilter filter, b3WorldTransform bodyTransform )
{ {
b3World* world = b3GetUnlockedWorld( bodyId.world0 ); b3World* world = b3GetUnlockedWorld( bodyId.world0 );
if ( world == NULL ) if ( world == NULL )
@ -811,8 +813,9 @@ int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int pla
return resultCount; return resultCount;
} }
b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover, b3Vec3 moverTranslation, b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover,
b3QueryFilter filter, b3WorldTransform bodyTransform1, b3WorldTransform bodyTransform2 ) b3Vec3 moverTranslation, b3QueryFilter filter,
b3WorldTransform bodyTransform1, b3WorldTransform bodyTransform2 )
{ {
b3BodyTOIResult result = { 0 }; b3BodyTOIResult result = { 0 };
result.fraction = 1.0f; result.fraction = 1.0f;
@ -874,7 +877,7 @@ b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b
B3_VALIDATE( output.state != b3_toiStateUnknown ); B3_VALIDATE( output.state != b3_toiStateUnknown );
// Mimic behavior in b3ContinuousQueryCallback. Ignore shapes that initially overlap. // Mimic behavior in b3ContinuousQueryCallback. Ignore shapes that initially overlap.
if (0.0f < output.fraction && output.fraction < result.fraction) if ( 0.0f < output.fraction && output.fraction < result.fraction )
{ {
input.maxFraction = output.fraction; input.maxFraction = output.fraction;
@ -911,7 +914,8 @@ static void b3UpdateIsotropicInertia( b3World* world, b3Body* body, b3BodySim* b
float tolerance = world->isotropicInertiaTolerance; float tolerance = world->isotropicInertiaTolerance;
if ( 2.0f * deviation > 3.0f * tolerance * tolerance * mean * mean ) bool fixedRotation = ( body->flags & b3_fixedRotation ) == b3_fixedRotation;
if ( fixedRotation == false && 2.0f * deviation > 3.0f * tolerance * tolerance * mean * mean )
{ {
body->flags |= b3_enableGyroscopicTorque; body->flags |= b3_enableGyroscopicTorque;
} }
@ -923,6 +927,27 @@ static void b3UpdateIsotropicInertia( b3World* world, b3Body* body, b3BodySim* b
b3SyncBodyFlags( world, body ); b3SyncBodyFlags( world, body );
} }
static void b3UpdateInverseInertia( b3BodySim* bodySim )
{
float det = b3Det( bodySim->inertia );
B3_ASSERT( det >= 0.0f );
if ( det > 0.0f && ( bodySim->flags & b3_fixedRotation ) != b3_fixedRotation )
{
// This call is faster than b3Invert
bodySim->invInertiaLocal = b3InvertT( bodySim->inertia );
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
bodySim->invInertiaWorld =
b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
}
else
{
bodySim->invInertiaLocal = b3Mat3_zero;
bodySim->invInertiaWorld = b3Mat3_zero;
}
}
void b3UpdateBodyMassData( b3World* world, b3Body* body ) void b3UpdateBodyMassData( b3World* world, b3Body* body )
{ {
b3BodySim* bodySim = b3GetBodySim( world, body ); b3BodySim* bodySim = b3GetBodySim( world, body );
@ -1024,17 +1049,7 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
b3StackFree( &world->stack, masses ); b3StackFree( &world->stack, masses );
masses = NULL; masses = NULL;
float det = b3Det( bodySim->inertia ); b3UpdateInverseInertia( bodySim );
B3_ASSERT( det >= 0.0f );
if ( det > 0.0f )
{
// This call is faster than b3Invert
bodySim->invInertiaLocal = b3InvertT( bodySim->inertia );
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
}
// Move center of mass. // Move center of mass.
b3Pos oldCenter = bodySim->center; b3Pos oldCenter = bodySim->center;
@ -1076,14 +1091,6 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
edgeKey = contact->edges[edgeIndex].nextKey; edgeKey = contact->edges[edgeIndex].nextKey;
} }
// Apply fixed rotation
if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation )
{
bodySim->inertia = b3Mat3_zero;
bodySim->invInertiaLocal = b3Mat3_zero;
bodySim->invInertiaWorld = b3Mat3_zero;
}
b3UpdateIsotropicInertia( world, body, bodySim ); b3UpdateIsotropicInertia( world, body, bodySim );
} }
@ -1160,7 +1167,8 @@ void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation )
bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter ); bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter );
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q ); b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); bodySim->invInertiaWorld =
b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
bodySim->rotation0 = bodySim->transform.q; bodySim->rotation0 = bodySim->transform.q;
bodySim->center0 = bodySim->center; bodySim->center0 = bodySim->center;
@ -1926,31 +1934,7 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
state->linearVelocity = b3Add( state->linearVelocity, deltaLinear ); state->linearVelocity = b3Add( state->linearVelocity, deltaLinear );
} }
float det = b3Det( bodySim->inertia ); b3UpdateInverseInertia( bodySim );
B3_ASSERT( det >= 0.0f );
if ( det > 0.0f )
{
// This call is faster than b3Invert
bodySim->invInertiaLocal = b3InvertT( bodySim->inertia );
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
}
else
{
bodySim->invInertiaLocal = b3Mat3_zero;
bodySim->invInertiaWorld = b3Mat3_zero;
}
// Apply fixed rotation
if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation )
{
bodySim->inertia = b3Mat3_zero;
bodySim->invInertiaLocal = b3Mat3_zero;
bodySim->invInertiaWorld = b3Mat3_zero;
}
b3UpdateIsotropicInertia( world, body, bodySim ); b3UpdateIsotropicInertia( world, body, bodySim );
// Update extents using supplied mass center. // Update extents using supplied mass center.
@ -2253,7 +2237,8 @@ void b3Body_Disable( b3BodyId bodyId )
continue; continue;
} }
B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet || joint->setIndex == b3_staticSet ); B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet ||
joint->setIndex == b3_staticSet );
// Remove joint from island // Remove joint from island
b3UnlinkJoint( world, joint ); b3UnlinkJoint( world, joint );
@ -2445,7 +2430,9 @@ void b3Body_SetMotionLocks( b3BodyId bodyId, b3MotionLocks locks )
if ( fixedRotation1 != fixedRotation2 ) if ( fixedRotation1 != fixedRotation2 )
{ {
b3UpdateBodyMassData( world, body ); b3BodySim* bodySim = b3GetBodySim( world, body );
b3UpdateInverseInertia( bodySim );
b3UpdateIsotropicInertia( world, body, bodySim );
} }
} }

View file

@ -66,12 +66,15 @@ enum b3BodyFlags
b3_enableGyroscopicTorque = 0x00010000, b3_enableGyroscopicTorque = 0x00010000,
// All lock flags
b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
// If all these flags are set then the body has fixed rotation // If all these flags are set then the body has fixed rotation
b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ, b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
// If all these flags are set then the body has fixed translation
b3_fixedTranslation = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ,
// All lock flags
b3_allLocks = b3_fixedRotation | b3_fixedTranslation,
// These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState. // These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState.
b3_bodyTransientFlags = b3_isSpeedCapped | b3_hadTimeOfImpact, b3_bodyTransientFlags = b3_isSpeedCapped | b3_hadTimeOfImpact,
}; };

View file

@ -449,20 +449,28 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi
void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias ); void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias );
void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias ); void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias );
void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB ); void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA,
b3WorldTransform transformB );
void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA, void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA,
b3WorldTransform transformB ); b3WorldTransform transformB );
void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA, void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA,
b3WorldTransform transformB, float scale ); b3WorldTransform transformB, float scale );
void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); float scale );
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA,
void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); b3WorldTransform transformB, float scale );
void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); float scale );
void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA,
b3WorldTransform transformB, float scale );
void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
float scale );
void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
float scale );
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
static inline b3Vec3 b3SolvePointConstraint( float mA, float mB, b3Matrix3 iA, b3Matrix3 iB, b3Vec3 rA, b3Vec3 rB, b3Vec3 rhs ) static inline b3Vec3 b3SolvePointConstraint( float mA, float mB, b3Matrix3 iA, b3Matrix3 iB, b3Vec3 rA, b3Vec3 rB,
b3Vec3 rhs )
{ {
float axy = rA.z * iA.cy.y - rA.y * iA.cz.y; float axy = rA.z * iA.cy.y - rA.y * iA.cz.y;
float axz = rA.z * iA.cy.z - rA.y * iA.cz.z; float axz = rA.z * iA.cy.z - rA.y * iA.cz.z;

View file

@ -572,6 +572,7 @@ typedef struct b3Primitive
b3AABB aabb; b3AABB aabb;
b3Vec3 center; b3Vec3 center;
int triangleIndex; int triangleIndex;
int8_t binIndex[3];
} b3Primitive; } b3Primitive;
_Static_assert( offsetof( b3Primitive, aabb.upperBound ) == 3 * sizeof( float ) && _Static_assert( offsetof( b3Primitive, aabb.upperBound ) == 3 * sizeof( float ) &&
@ -694,6 +695,10 @@ static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives )
B3_ASSERT( 0 <= indexY && indexY < B3_BIN_COUNT ); B3_ASSERT( 0 <= indexY && indexY < B3_BIN_COUNT );
B3_ASSERT( 0 <= indexZ && indexZ < B3_BIN_COUNT ); B3_ASSERT( 0 <= indexZ && indexZ < B3_BIN_COUNT );
primitives[i].binIndex[0] = (int8_t)indexX;
primitives[i].binIndex[1] = (int8_t)indexY;
primitives[i].binIndex[2] = (int8_t)indexZ;
const float* p = &primitives[i].aabb.lowerBound.x; const float* p = &primitives[i].aabb.lowerBound.x;
b3FloatW4 lower = b3LoadW4( p ); b3FloatW4 lower = b3LoadW4( p );
b3FloatW4 upper = b3LoadW4( p + 2 ); b3FloatW4 upper = b3LoadW4( p + 2 );
@ -772,14 +777,11 @@ static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives )
if ( bestBucket >= 0 ) if ( bestBucket >= 0 )
{ {
int axis = split.axis; int axis = split.axis;
float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) /
( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) );
int splitIndex = 0; int splitIndex = 0;
for ( int i = 0; i < count; ++i ) for ( int i = 0; i < count; ++i )
{ {
b3Vec3 center = primitives[i].center; int index = primitives[i].binIndex[axis];
int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) );
if ( index <= bestBucket ) if ( index <= bestBucket )
{ {

View file

@ -1830,10 +1830,10 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
// Make a temporary copy of previous manifolds // Make a temporary copy of previous manifolds
int oldManifoldCount = contact->manifoldCount; int oldManifoldCount = contact->manifoldCount;
b3Manifold* oldManifolds = NULL; B3_ASSERT( oldManifoldCount <= B3_MAX_MANIFOLDS );
b3Manifold oldManifolds[B3_MAX_MANIFOLDS];
if ( oldManifoldCount > 0 ) if ( oldManifoldCount > 0 )
{ {
oldManifolds = b3Bump( &arena, oldManifoldCount * sizeof( b3Manifold ) );
memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) ); memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) );
} }
@ -1850,12 +1850,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
memset( contact->manifolds, 0, contact->manifoldCount * sizeof( b3Manifold ) ); memset( contact->manifolds, 0, contact->manifoldCount * sizeof( b3Manifold ) );
} }
bool* consumed = NULL; bool consumed[B3_MAX_MANIFOLDS] = { 0 };
if ( oldManifoldCount > 0 )
{
consumed = b3Bump( &arena, oldManifoldCount * sizeof( bool ) );
memset( consumed, 0, oldManifoldCount * sizeof( bool ) );
}
b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q ); b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q );
b3Vec3 offsetA = b3SubPos( xfB.p, xfA.p ); b3Vec3 offsetA = b3SubPos( xfB.p, xfA.p );

View file

@ -253,7 +253,10 @@ b3WorldId b3CreateWorld( const b3WorldDef* def )
world->stack = b3CreateStack( 2048 ); world->stack = b3CreateStack( 2048 );
b3Array_Reserve( world->manifoldAllocators, 16 ); for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i )
{
world->manifoldAllocators[i] = b3CreateBlockAllocator( ( i + 1 ) * (int)sizeof( b3Manifold ), 0 );
}
world->manifoldAllocatorMutex = b3CreateMutex(); world->manifoldAllocatorMutex = b3CreateMutex();
b3CreateBroadPhase( &world->broadPhase, &def->capacity ); b3CreateBroadPhase( &world->broadPhase, &def->capacity );
@ -540,11 +543,10 @@ void b3DestroyWorld( b3WorldId worldId )
b3DestroyIdPool( &world->islandIdPool ); b3DestroyIdPool( &world->islandIdPool );
b3DestroyIdPool( &world->solverSetIdPool ); b3DestroyIdPool( &world->solverSetIdPool );
for ( int i = 0; i < world->manifoldAllocators.count; ++i ) for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i )
{ {
b3DestroyBlockAllocator( world->manifoldAllocators.data + i ); b3DestroyBlockAllocator( world->manifoldAllocators + i );
} }
b3Array_Destroy( world->manifoldAllocators );
b3DestroyMutex( world->manifoldAllocatorMutex ); b3DestroyMutex( world->manifoldAllocatorMutex );
b3DestroyStack( &world->stack ); b3DestroyStack( &world->stack );
@ -2846,20 +2848,18 @@ static uint64_t b3GetWorldByteCount( b3World* world, bool log )
} }
// Manifold block allocators, one per manifold point count // Manifold block allocators, one per manifold point count
int manifoldArrayBytes = b3Array_ByteCount( world->manifoldAllocators );
int manifoldBlockBytes = 0; int manifoldBlockBytes = 0;
for ( int i = 0; i < world->manifoldAllocators.count; ++i ) for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i )
{ {
b3BlockAllocator* allocator = world->manifoldAllocators.data + i; b3BlockAllocator* allocator = world->manifoldAllocators + i;
manifoldBlockBytes += b3Array_ByteCount( allocator->blocks ); manifoldBlockBytes += b3Array_ByteCount( allocator->blocks );
manifoldBlockBytes += allocator->blocks.count * B3_BLOCK_SIZE * allocator->elementSize; manifoldBlockBytes += allocator->blocks.count * B3_BLOCK_SIZE * allocator->elementSize;
} }
total += (uint64_t)manifoldArrayBytes + manifoldBlockBytes; total += (uint64_t)manifoldBlockBytes;
if ( log ) if ( log )
{ {
b3Log( "manifold allocators" ); b3Log( "manifold allocators" );
b3Log( "allocator array: %d", manifoldArrayBytes );
b3Log( "blocks: %d", manifoldBlockBytes ); b3Log( "blocks: %d", manifoldBlockBytes );
} }

View file

@ -131,7 +131,7 @@ typedef struct b3World
b3ConstraintGraph constraintGraph; b3ConstraintGraph constraintGraph;
// Manifold allocators have one allocator for each manifold count. // Manifold allocators have one allocator for each manifold count.
b3Array( b3BlockAllocator ) manifoldAllocators; b3BlockAllocator manifoldAllocators[B3_MAX_MANIFOLDS];
b3Mutex* manifoldAllocatorMutex; b3Mutex* manifoldAllocatorMutex;
// The body id pool is used to allocate and recycle body ids. Body ids // The body id pool is used to allocate and recycle body ids. Body ids
@ -353,19 +353,9 @@ static inline b3Manifold* b3AllocateManifolds( b3World* world, int count )
B3_ASSERT( count <= B3_MAX_MANIFOLDS ); B3_ASSERT( count <= B3_MAX_MANIFOLDS );
int index = count - 1; // Need lock because this is called from the parallel narrow phase.
// Need lock because this is called from the parallel narrow phase
b3LockMutex( world->manifoldAllocatorMutex ); b3LockMutex( world->manifoldAllocatorMutex );
int currentCount = world->manifoldAllocators.count; b3Manifold* manifolds = b3AllocateElement( world->manifoldAllocators + count - 1 );
for ( int i = currentCount; i < count; ++i )
{
b3BlockAllocator allocator = b3CreateBlockAllocator( ( i + 1 ) * sizeof( b3Manifold ), 2 * B3_BLOCK_SIZE );
b3Array_Push( world->manifoldAllocators, allocator );
}
b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index );
b3Manifold* manifolds = (b3Manifold*)b3AllocateElement( allocator );
b3UnlockMutex( world->manifoldAllocatorMutex ); b3UnlockMutex( world->manifoldAllocatorMutex );
memset( manifolds, 0, count * sizeof( b3Manifold ) ); memset( manifolds, 0, count * sizeof( b3Manifold ) );
return manifolds; return manifolds;
@ -380,9 +370,7 @@ static inline void b3FreeManifolds( b3World* world, b3Manifold* manifolds, int c
B3_ASSERT( count <= B3_MAX_MANIFOLDS ); B3_ASSERT( count <= B3_MAX_MANIFOLDS );
int index = count - 1;
b3LockMutex( world->manifoldAllocatorMutex ); b3LockMutex( world->manifoldAllocatorMutex );
b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index ); b3FreeElement( world->manifoldAllocators + count - 1, manifolds );
b3FreeElement( allocator, manifolds );
b3UnlockMutex( world->manifoldAllocatorMutex ); b3UnlockMutex( world->manifoldAllocatorMutex );
} }

View file

@ -5,9 +5,9 @@
#include "joint.h" #include "joint.h"
#include "math_internal.h" #include "math_internal.h"
#include "physics_world.h" #include "physics_world.h"
#include "recording.h"
#include "solver.h" #include "solver.h"
#include "solver_set.h" #include "solver_set.h"
#include "recording.h"
// needed for dll export // needed for dll export
#include "box3d/box3d.h" #include "box3d/box3d.h"
@ -240,10 +240,10 @@ b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base )
b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q ); b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q );
// These are needed for warm starting // These are needed for warm starting
joint->perpAxisX = b3MulSV( joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ),
0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
joint->perpAxisY = b3MulSV( joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ),
0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
b3Vec3 angularImpulse = b3Vec3 angularImpulse =
@ -251,8 +251,8 @@ b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base )
angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ ); angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ );
// todo add pivot torque // todo add pivot torque
b3Vec3 impulse = b3MulAdd( angularImpulse, b3Vec3 impulse = b3MulAdd(
joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, axis ); angularImpulse, joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, axis );
b3Vec3 torque = b3MulSV( world->inv_h, impulse ); b3Vec3 torque = b3MulSV( world->inv_h, impulse );
return torque; return torque;
} }
@ -263,8 +263,8 @@ void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context )
b3World* world = context->world; b3World* world = context->world;
b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA );
b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB );
B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex );
@ -273,8 +273,8 @@ void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context )
int localIndexA = bodyA->localIndex; int localIndexA = bodyA->localIndex;
int localIndexB = bodyB->localIndex; int localIndexB = bodyB->localIndex;
b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA );
b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB );
base->invMassA = bodySimA->invMass; base->invMassA = bodySimA->invMass;
base->invMassB = bodySimB->invMass; base->invMassB = bodySimB->invMass;
@ -312,10 +312,10 @@ void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context )
{ {
// These are needed for warm starting // These are needed for warm starting
joint->perpAxisX = b3MulSV( joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ),
0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
joint->perpAxisY = b3MulSV( joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ),
0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
} }
joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h );
@ -432,7 +432,8 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
float massScale = joint->springSoftness.massScale; float massScale = joint->springSoftness.massScale;
float impulseScale = joint->springSoftness.impulseScale; float impulseScale = joint->springSoftness.impulseScale;
float deltaImpulse = -massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * joint->springImpulse; float deltaImpulse =
-massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * joint->springImpulse;
joint->springImpulse += deltaImpulse; joint->springImpulse += deltaImpulse;
axialCdot += k * deltaImpulse; axialCdot += k * deltaImpulse;
@ -444,7 +445,8 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
float cdot = axialCdot - joint->motorSpeed; float cdot = axialCdot - joint->motorSpeed;
float oldImpulse = joint->motorImpulse; float oldImpulse = joint->motorImpulse;
float maxImpulse = joint->maxMotorTorque * context->h; float maxImpulse = joint->maxMotorTorque * context->h;
float deltaImpulse = b3ClampFloat( -joint->axialMass * cdot, -maxImpulse - oldImpulse, maxImpulse - oldImpulse ); float deltaImpulse =
b3ClampFloat( -joint->axialMass * cdot, -maxImpulse - oldImpulse, maxImpulse - oldImpulse );
joint->motorImpulse = oldImpulse + deltaImpulse; joint->motorImpulse = oldImpulse + deltaImpulse;
axialCdot += k * deltaImpulse; axialCdot += k * deltaImpulse;
@ -476,8 +478,8 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
} }
float oldImpulse = joint->lowerImpulse; float oldImpulse = joint->lowerImpulse;
float deltaImpulse = float deltaImpulse = b3MaxFloat(
b3MaxFloat( -massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * oldImpulse, -oldImpulse ); -massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * oldImpulse, -oldImpulse );
joint->lowerImpulse = oldImpulse + deltaImpulse; joint->lowerImpulse = oldImpulse + deltaImpulse;
axialCdot += k * deltaImpulse; axialCdot += k * deltaImpulse;
@ -505,8 +507,8 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
// sign flipped on Cdot // sign flipped on Cdot
float cdot = -axialCdot; float cdot = -axialCdot;
float oldImpulse = joint->upperImpulse; float oldImpulse = joint->upperImpulse;
float deltaImpulse = float deltaImpulse = b3MaxFloat(
b3MaxFloat( -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse, -oldImpulse ); -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse, -oldImpulse );
joint->upperImpulse = oldImpulse + deltaImpulse; joint->upperImpulse = oldImpulse + deltaImpulse;
// sign flipped on applied impulse // sign flipped on applied impulse
@ -534,10 +536,10 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
} }
// Collinearity constraint as 2-by-2 // Collinearity constraint as 2-by-2
b3Vec3 perpAxisX = b3Vec3 perpAxisX = b3MulSV(
b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
b3Vec3 perpAxisY = b3Vec3 perpAxisY = b3MulSV(
b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
joint->perpAxisX = perpAxisX; joint->perpAxisX = perpAxisX;
joint->perpAxisY = perpAxisY; joint->perpAxisY = perpAxisY;
@ -606,22 +608,29 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
} }
} }
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ) void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
float scale )
{ {
b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA ); b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
float length1 = 0.1f * scale; float length1 = 0.1f * scale;
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed, draw->DrawSegmentFcn( frameA.p,
draw->context ); b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ),
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen, b3_colorRed, draw->context );
draw->context ); draw->DrawSegmentFcn( frameA.p,
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ),
draw->context ); b3_colorGreen, draw->context );
draw->DrawSegmentFcn( frameA.p,
b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ),
b3_colorBlue, draw->context );
b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB ); b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
b3RevoluteJoint* joint = &base->revoluteJoint; b3RevoluteJoint* joint = &base->revoluteJoint;
enum { kSliceCount = 16 }; enum
{
kSliceCount = 16
};
// Twist limit // Twist limit
if ( joint->enableLimit ) if ( joint->enableLimit )
@ -657,8 +666,8 @@ void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform
{ {
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context ); draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );
} }
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan, draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ),
draw->context ); b3_colorCyan, draw->context );
} }
float twistAngle = b3GetTwistAngle( relQ ); float twistAngle = b3GetTwistAngle( relQ );

View file

@ -182,6 +182,13 @@ static int DeferredMassExtents( void )
// Diagonal inertia with inverses that are exact in float, so tolerances stay tight. // Diagonal inertia with inverses that are exact in float, so tolerances stay tight.
static const b3Matrix3 kDiagInertia = { { 2.0f, 0.0f, 0.0f }, { 0.0f, 4.0f, 0.0f }, { 0.0f, 0.0f, 8.0f } }; static const b3Matrix3 kDiagInertia = { { 2.0f, 0.0f, 0.0f }, { 0.0f, 4.0f, 0.0f }, { 0.0f, 0.0f, 8.0f } };
static bool HasGyroscopicTorque( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
return ( body->flags & b3_enableGyroscopicTorque ) != 0;
}
static int SetMassDataRoundTrip( void ) static int SetMassDataRoundTrip( void )
{ {
b3WorldDef worldDef = b3DefaultWorldDef(); b3WorldDef worldDef = b3DefaultWorldDef();
@ -277,8 +284,8 @@ static int SetMassDataWorldInertiaRotated( void )
return 0; return 0;
} }
// Fixed rotation must leave the mass intact but drive the whole angular inertia to zero, even when the // Fixed rotation zeroes only the inverse inertia. The caller's tensor survives so unlocking restores it
// caller hands in a real tensor. // without recomputing mass from the shapes.
static int SetMassDataFixedRotation( void ) static int SetMassDataFixedRotation( void )
{ {
b3WorldDef worldDef = b3DefaultWorldDef(); b3WorldDef worldDef = b3DefaultWorldDef();
@ -298,9 +305,9 @@ static int SetMassDataFixedRotation( void )
ENSURE_SMALL( b3Body_GetInverseMass( bodyId ) - 0.2f, 1e-6f ); ENSURE_SMALL( b3Body_GetInverseMass( bodyId ) - 0.2f, 1e-6f );
b3Matrix3 localInertia = b3Body_GetLocalRotationalInertia( bodyId ); b3Matrix3 localInertia = b3Body_GetLocalRotationalInertia( bodyId );
ENSURE_SMALL( localInertia.cx.x, 1e-6f ); ENSURE_SMALL( localInertia.cx.x - 2.0f, 1e-6f );
ENSURE_SMALL( localInertia.cy.y, 1e-6f ); ENSURE_SMALL( localInertia.cy.y - 4.0f, 1e-6f );
ENSURE_SMALL( localInertia.cz.z, 1e-6f ); ENSURE_SMALL( localInertia.cz.z - 8.0f, 1e-6f );
b3Matrix3 invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId ); b3Matrix3 invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
ENSURE_SMALL( invWorld.cx.x, 1e-6f ); ENSURE_SMALL( invWorld.cx.x, 1e-6f );
@ -308,9 +315,36 @@ static int SetMassDataFixedRotation( void )
ENSURE_SMALL( invWorld.cz.z, 1e-6f ); ENSURE_SMALL( invWorld.cz.z, 1e-6f );
b3MassData md = b3Body_GetMassData( bodyId ); b3MassData md = b3Body_GetMassData( bodyId );
ENSURE_SMALL( md.inertia.cx.x, 1e-6f ); ENSURE_SMALL( md.inertia.cx.x - 2.0f, 1e-6f );
ENSURE_SMALL( md.inertia.cy.y, 1e-6f ); ENSURE_SMALL( md.inertia.cy.y - 4.0f, 1e-6f );
ENSURE_SMALL( md.inertia.cz.z, 1e-6f ); ENSURE_SMALL( md.inertia.cz.z - 8.0f, 1e-6f );
// The tensor is anisotropic but the body cannot rotate
ENSURE( HasGyroscopicTorque( bodyId ) == false );
b3Body_SetMotionLocks( bodyId, (b3MotionLocks){ 0 } );
ENSURE_SMALL( b3Body_GetMass( bodyId ) - 5.0f, 1e-6f );
invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
ENSURE_SMALL( invWorld.cx.x - 0.5f, 1e-6f );
ENSURE_SMALL( invWorld.cy.y - 0.25f, 1e-6f );
ENSURE_SMALL( invWorld.cz.z - 0.125f, 1e-6f );
md = b3Body_GetMassData( bodyId );
ENSURE_SMALL( md.inertia.cx.x - 2.0f, 1e-6f );
ENSURE_SMALL( md.inertia.cy.y - 4.0f, 1e-6f );
ENSURE_SMALL( md.inertia.cz.z - 8.0f, 1e-6f );
ENSURE( HasGyroscopicTorque( bodyId ) );
b3Body_SetMotionLocks( bodyId, bodyDef.motionLocks );
invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
ENSURE_SMALL( invWorld.cx.x, 1e-6f );
ENSURE_SMALL( invWorld.cy.y, 1e-6f );
ENSURE_SMALL( invWorld.cz.z, 1e-6f );
ENSURE( HasGyroscopicTorque( bodyId ) == false );
b3DestroyWorld( worldId ); b3DestroyWorld( worldId );
return 0; return 0;
@ -540,13 +574,6 @@ static b3BodyId CreateSpinningHullBody( b3WorldId worldId, float x, const b3Hull
return bodyId; return bodyId;
} }
static bool HasGyroscopicTorque( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
return ( body->flags & b3_enableGyroscopicTorque ) != 0;
}
// Bodies within the isotropic tolerance skip the gyroscopic solve, so in free flight the world // Bodies within the isotropic tolerance skip the gyroscopic solve, so in free flight the world
// angular velocity is untouched. Anisotropic bodies precess. // angular velocity is untouched. Anisotropic bodies precess.
static int IsotropicInertiaTolerance( void ) static int IsotropicInertiaTolerance( void )