Gyro tolerance (#191)

This commit is contained in:
Erin Catto 2026-10-15 23:30:56 -07:00 • committed by GitHub
commit efca0c184d
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
23 changed files with 420 additions and 63 deletions

View file

@ -214,6 +214,13 @@ B3_API void b3World_SetMaximumLinearSpeed( b3WorldId worldId, float maximumLinea
/// Get the maximum linear speed. Usually in m/s.
B3_API float b3World_GetMaximumLinearSpeed( b3WorldId worldId );
/// Set the tolerance for gyroscopic torque. [0, 1]
/// @note modifying this only affects new bodies or bodies that get their mass updated.
B3_API void b3World_SetIsotropicInertiaTolerance( b3WorldId worldId, float tolerance );
/// Get the tolerance for gyroscopic torque. [0, 1]
B3_API float b3World_GetIsotropicInertiaTolerance( b3WorldId worldId );
/// Enable/disable constraint warm starting. Advanced feature for testing. Disabling
/// warm starting greatly reduces stability and provides no performance gain.
B3_API void b3World_EnableWarmStarting( b3WorldId worldId, bool flag );

View file

@ -173,6 +173,10 @@ typedef struct b3WorldDef
/// Maximum linear speed. Usually meters per second.
float maximumLinearSpeed;
/// Use this to disable gyroscopic torque on sphere-like objects automatically.
/// Range is [0, 1]. A sphere is 0 and a 1x1x2 box is 0.5.
float isotropicInertiaTolerance;
/// Optional mixing callback for friction. The default uses sqrt(frictionA * frictionB).
b3FrictionCallback* frictionCallback;

View file

@ -583,6 +583,107 @@ public:
static int sampleGyroscopicPrecession = RegisterSample( "Bodies", "Gyroscopic Precession", GyroscopicPrecession::Create );
// This show that gyroscopic torque is necessary for plausible behavior on long skinny objects.
class SpinningCapsule : public Sample
{
public:
explicit SpinningCapsule( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 30.0f, 25.0f, 15.0f, { 0.0f, 1.0f, 0.0f } );
}
m_length = 4.0f;
m_tolerance = b3World_GetIsotropicInertiaTolerance( m_worldId );
CreateScene();
}
void CreateScene()
{
// The tolerance only applies to bodies created after it is set
b3Capacity capacity = {};
CreateWorld( &capacity );
b3World_SetIsotropicInertiaTolerance( m_worldId, m_tolerance );
AddGroundBox( 20.0f );
float radius = 0.2f;
float halfLength = 0.5f * m_length;
b3Capsule capsule = { { 0.0f, -halfLength, 0.0f }, { 0.0f, halfLength, 0.0f }, radius };
// Stand on the lower cap, tilted about it
b3Quat rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 15.0f * B3_PI / 180.0f );
b3Vec3 axis = b3RotateVector( rotation, b3Vec3_axisY );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = b3OffsetPos( { 0.0f, radius, 0.0f }, halfLength * axis );
bodyDef.rotation = rotation;
bodyDef.angularVelocity = 20.0f * axis;
m_capsuleId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3CreateCapsuleShape( m_capsuleId, &shapeDef, &capsule );
}
bool DrawControls() override
{
bool changed = false;
ImGui::PushItemWidth( 10.0f * ImGui::GetFontSize() );
changed |= ImGui::SliderFloat( "Length", &m_length, 0.0f, 8.0f, "%.1f" );
changed |= ImGui::SliderFloat( "Tolerance", &m_tolerance, 0.0f, 1.0f, "%.3f" );
ImGui::PopItemWidth();
if ( ImGui::Button( "Reset" ) || changed )
{
CreateScene();
}
return true;
}
void Step() override
{
Sample::Step();
// Same measure the world uses to skip the gyroscopic solve: the deviatoric part of the inertia
// relative to its mean.
b3Matrix3 inertia = b3Body_GetLocalRotationalInertia( m_capsuleId );
float mean = ( inertia.cx.x + inertia.cy.y + inertia.cz.z ) / 3.0f;
float dx = inertia.cx.x - mean;
float dy = inertia.cy.y - mean;
float dz = inertia.cz.z - mean;
float offDiagonal = inertia.cy.x * inertia.cy.x + inertia.cz.x * inertia.cz.x + inertia.cz.y * inertia.cz.y;
float deviation = dx * dx + dy * dy + dz * dz + 2.0f * offDiagonal;
float anisotropy = mean > 0.0f ? sqrtf( 2.0f * deviation / 3.0f ) / mean : 0.0f;
bool isGyroscopic = anisotropy > m_tolerance;
b3Pos center = b3Body_GetWorldCenter( m_capsuleId );
b3Vec3 axis = b3RotateVector( b3Body_GetRotation( m_capsuleId ), b3Vec3_axisY );
b3Vec3 omega = b3Body_GetAngularVelocity( m_capsuleId );
float tilt = acosf( b3ClampFloat( b3AbsFloat( axis.y ), 0.0f, 1.0f ) );
DrawLine( center, b3OffsetPos( center, 0.2f * omega ), MakeColor( b3_colorYellow ) );
DrawTextLine( "anisotropy %.2f, gyroscopic torque %s", anisotropy, isGyroscopic ? "on" : "off" );
DrawTextLine( "tilt %.0f deg, spin %.1f rad/s", ( 180.0f / B3_PI ) * tilt, b3Dot( omega, axis ) );
}
static Sample* Create( SampleContext* context )
{
return new SpinningCapsule( context );
}
b3BodyId m_capsuleId;
float m_length;
float m_tolerance;
};
static int sampleSpinningCapsule = RegisterSample( "Bodies", "Spinning Capsule", SpinningCapsule::Create );
class Weeble : public Sample
{
public:

View file

@ -254,6 +254,7 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def )
bodySim->force = b3Vec3_zero;
bodySim->torque = b3Vec3_zero;
bodySim->invMass = 0.0f;
bodySim->inertia = b3Mat3_zero;
bodySim->invInertiaLocal = b3Mat3_zero;
bodySim->minExtent = B3_HUGE;
bodySim->maxExtent = b3Vec3_zero;
@ -266,7 +267,7 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def )
bodySim->flags |= def->allowFastRotation ? b3_allowFastRotation : 0;
bodySim->flags |= def->type == b3_dynamicBody ? b3_dynamicFlag : 0;
bodySim->flags |= def->enableSleep ? b3_enableSleep : 0;
bodySim->flags |= def->enableContactRecycling ? b3_bodyEnableContactRecycling : 0;
bodySim->flags |= def->enableContactRecycling ? b3_enableContactRecycling : 0;
if ( setId == b3_awakeSet )
{
@ -309,7 +310,6 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def )
body->sleepVelocity = 0.0f;
body->safetyFactor = def->safetyFactor;
body->mass = 0.0f;
body->inertia = b3Mat3_zero;
body->nameId = b3AddName( &world->names, def->name );
body->type = def->type;
body->flags = bodySim->flags;
@ -892,18 +892,46 @@ b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b
return result;
}
// https://en.wikipedia.org/wiki/Cauchy_stress_tensor#Stress_deviator_tensor
static void b3UpdateIsotropicInertia( b3World* world, b3Body* body, b3BodySim* bodySim )
{
b3Matrix3 inertia = bodySim->inertia;
float mean = ( inertia.cx.x + inertia.cy.y + inertia.cz.z ) / 3.0f;
float dx = inertia.cx.x - mean;
float dy = inertia.cy.y - mean;
float dz = inertia.cz.z - mean;
float offDiagonal = inertia.cy.x * inertia.cy.x + inertia.cz.x * inertia.cz.x + inertia.cz.y * inertia.cz.y;
// This is the square of the Frobenius matrix norm.
// https://en.wikipedia.org/wiki/Matrix_norm
float deviation = dx * dx + dy * dy + dz * dz + 2.0f * offDiagonal;
float tolerance = world->isotropicInertiaTolerance;
if ( 2.0f * deviation > 3.0f * tolerance * tolerance * mean * mean )
{
body->flags |= b3_enableGyroscopicTorque;
}
else
{
body->flags &= ~b3_enableGyroscopicTorque;
}
b3SyncBodyFlags( world, body );
}
void b3UpdateBodyMassData( b3World* world, b3Body* body )
{
b3BodySim* bodySim = b3GetBodySim( world, body );
// Mass is no longer dirty
body->flags &= ~b3_dirtyMass;
body->flags &= ~b3_enableGyroscopicTorque;
b3SyncBodyFlags( world, body );
// Compute mass data from shapes. Each shape has its own density.
body->mass = 0.0f;
body->inertia = b3Mat3_zero;
bodySim->inertia = b3Mat3_zero;
bodySim->invMass = 0.0f;
bodySim->invInertiaLocal = b3Mat3_zero;
bodySim->invInertiaWorld = b3Mat3_zero;
@ -987,19 +1015,19 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
// Shift to center of mass. This is safe because it can only increase.
b3Vec3 offset = b3Sub( localCenter, massData.center );
b3Matrix3 inertia = b3AddMM( massData.inertia, b3Steiner( massData.mass, offset ) );
body->inertia = b3AddMM( body->inertia, inertia );
bodySim->inertia = b3AddMM( bodySim->inertia, inertia );
}
b3StackFree( &world->stack, masses );
masses = NULL;
float det = b3Det( body->inertia );
float det = b3Det( bodySim->inertia );
B3_ASSERT( det >= 0.0f );
if ( det > 0.0f )
{
// This call is faster than b3Invert
bodySim->invInertiaLocal = b3InvertT( body->inertia );
bodySim->invInertiaLocal = b3InvertT( bodySim->inertia );
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
@ -1048,10 +1076,12 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
// Apply fixed rotation
if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation )
{
body->inertia = b3Mat3_zero;
bodySim->inertia = b3Mat3_zero;
bodySim->invInertiaLocal = b3Mat3_zero;
bodySim->invInertiaWorld = b3Mat3_zero;
}
b3UpdateIsotropicInertia( world, body, bodySim );
}
b3Pos b3Body_GetPosition( b3BodyId bodyId )
@ -1818,7 +1848,8 @@ b3Matrix3 b3Body_GetLocalRotationalInertia( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
return body->inertia;
b3BodySim* bodySim = b3GetBodySim( world, body );
return bodySim->inertia;
}
float b3Body_GetInverseMass( b3BodyId bodyId )
@ -1875,7 +1906,6 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
b3SyncBodyFlags( world, body );
body->mass = massData.mass;
body->inertia = massData.inertia;
bodySim->localCenter = massData.center;
b3Pos oldCenter = bodySim->center;
@ -1883,6 +1913,7 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
bodySim->center = center;
bodySim->center0 = center;
bodySim->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f;
bodySim->inertia = massData.inertia;
// Update center of mass velocity
b3BodyState* state = b3GetBodyState( world, body );
@ -1892,13 +1923,13 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
state->linearVelocity = b3Add( state->linearVelocity, deltaLinear );
}
float det = b3Det( body->inertia );
float det = b3Det( bodySim->inertia );
B3_ASSERT( det >= 0.0f );
if ( det > 0.0f )
{
// This call is faster than b3Invert
bodySim->invInertiaLocal = b3InvertT( body->inertia );
bodySim->invInertiaLocal = b3InvertT( bodySim->inertia );
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
@ -1912,11 +1943,13 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
// Apply fixed rotation
if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation )
{
body->inertia = b3Mat3_zero;
bodySim->inertia = b3Mat3_zero;
bodySim->invInertiaLocal = b3Mat3_zero;
bodySim->invInertiaWorld = b3Mat3_zero;
}
b3UpdateIsotropicInertia( world, body, bodySim );
// Update extents using supplied mass center.
bodySim->minExtent = B3_HUGE;
bodySim->maxExtent = b3Vec3_zero;
@ -1949,7 +1982,7 @@ b3MassData b3Body_GetMassData( b3BodyId bodyId )
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
b3BodySim* bodySim = b3GetBodySim( world, body );
b3MassData massData = { body->mass, bodySim->localCenter, body->inertia };
b3MassData massData = { body->mass, bodySim->localCenter, bodySim->inertia };
return massData;
}
@ -2500,15 +2533,15 @@ void b3Body_EnableContactRecycling( b3BodyId bodyId, bool flag )
B3_REC( world, BodyEnableContactRecycling, bodyId, flag );
uint32_t newFlag = flag ? b3_bodyEnableContactRecycling : 0;
uint32_t newFlag = flag ? b3_enableContactRecycling : 0;
b3Body* body = b3GetBodyFullId( world, bodyId );
if ( ( body->flags & b3_bodyEnableContactRecycling ) == newFlag )
if ( ( body->flags & b3_enableContactRecycling ) == newFlag )
{
return;
}
body->flags &= ~b3_bodyEnableContactRecycling;
body->flags &= ~b3_enableContactRecycling;
body->flags |= newFlag;
b3SyncBodyFlags( world, body );
@ -2518,7 +2551,7 @@ bool b3Body_IsContactRecyclingEnabled( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
return ( body->flags & b3_bodyEnableContactRecycling ) != 0;
return ( body->flags & b3_enableContactRecycling ) != 0;
}
void b3Body_EnableHitEvents( b3BodyId bodyId, bool flag )

View file

@ -58,12 +58,14 @@ enum b3BodyFlags
b3_enableSleep = 0x00002000,
b3_bodyEnableContactRecycling = 0x00004000,
b3_enableContactRecycling = 0x00004000,
// The user deferred mass computation via the updateBodyMass shape option and mass
// data still hasn't been set.
b3_dirtyMass = 0x00008000,
b3_enableGyroscopicTorque = 0x00010000,
// All lock flags
b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
@ -114,9 +116,6 @@ typedef struct b3Body
float safetyFactor;
float mass;
// local space inertia
b3Matrix3 inertia;
// this is used to adjust the fellAsleep flag in the body move array
int bodyMoveIndex;
@ -213,6 +212,9 @@ typedef struct b3BodySim
float invMass;
// Local space inertia
b3Matrix3 inertia;
// Rotational inertia about the center of mass. The world space inverse inertia tensor
// must be updated whenever the body rotation is modified.
b3Matrix3 invInertiaLocal;

View file

@ -5,7 +5,6 @@
#include "algorithm.h"
#include "body.h"
#include "compound.h"
#include "island.h"
#include "manifold.h"
#include "physics_world.h"
@ -250,7 +249,7 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil
}
// Disabling speculative collision also disables recycling.
if ( ( bodyA->flags & b3_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b3_bodyEnableContactRecycling ) != 0 &&
if ( ( bodyA->flags & b3_enableContactRecycling ) != 0 && ( bodyB->flags & b3_enableContactRecycling ) != 0 &&
( contact->flags & b3_enableSpeculativePoints ) != 0 )
{
contact->flags |= b3_contactRecycleFlag;

View file

@ -394,16 +394,6 @@ static inline b3Matrix3 b3MakeDiagonalMatrix( float a, float b, float c )
return (b3Matrix3){ { a, 0.0f, 0.0f }, { 0.0f, b, 0.0f }, { 0.0f, 0.0f, c } };
}
static inline b3Matrix3 b3Skew( b3Vec3 v )
{
b3Matrix3 out;
out.cx = (b3Vec3){ 0, v.z, -v.y };
out.cy = (b3Vec3){ -v.z, 0, v.x };
out.cz = (b3Vec3){ v.y, -v.x, 0 };
return out;
}
static inline b3Plane b3NormalizePlane( b3Plane plane )
{
float invLength = 1.0f / b3Length( plane.normal );

View file

@ -360,7 +360,6 @@ void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context )
b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) );
//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, b3Add( cdot, bias ) );
b3Vec3 oldImpulse = joint->linearSpringImpulse;
@ -386,7 +385,7 @@ void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context )
{
b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) );
cdot = b3Sub( cdot, joint->linearVelocity );
//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, cdot );
b3Vec3 impulse = b3Neg( b );

View file

@ -332,6 +332,7 @@ b3WorldId b3CreateWorld( const b3WorldDef* def )
world->restitutionIterations = b3ClampInt( def->restitutionIterations, 0, B3_MAX_RESTITUTION_ITERATIONS );
world->enableRestitutionPropagation = def->enableRestitutionPropagation;
world->maxLinearSpeed = def->maximumLinearSpeed;
world->isotropicInertiaTolerance = b3ClampFloat( def->isotropicInertiaTolerance, 0.0f, 1.0f );
world->contactSpeed = def->contactSpeed;
world->contactHertz = def->contactHertz;
world->contactDampingRatio = def->contactDampingRatio;
@ -2215,6 +2216,27 @@ float b3World_GetMaximumLinearSpeed( b3WorldId worldId )
return world->maxLinearSpeed;
}
void b3World_SetIsotropicInertiaTolerance( b3WorldId worldId, float tolerance )
{
B3_ASSERT( b3IsValidFloat( tolerance ) );
b3World* world = b3GetUnlockedWorldFromId( worldId );
if ( world == NULL )
{
return;
}
B3_REC( world, WorldSetIsotropicInertiaTolerance, worldId, tolerance );
world->isotropicInertiaTolerance = b3ClampFloat( tolerance, 0.0f, 1.0f );
}
float b3World_GetIsotropicInertiaTolerance( b3WorldId worldId )
{
b3World* world = b3GetWorldFromId( worldId );
return world->isotropicInertiaTolerance;
}
b3Profile b3World_GetProfile( b3WorldId worldId )
{
b3World* world = b3GetUnlockedWorldFromId( worldId );

View file

@ -238,6 +238,7 @@ typedef struct b3World
int restitutionIterations;
bool enableRestitutionPropagation;
float maxLinearSpeed;
float isotropicInertiaTolerance;
float contactSpeed;
float contactHertz;
float contactDampingRatio;

View file

@ -52,7 +52,7 @@ typedef struct b3World b3World;
// Minor tracks op-stream additions that keep the 48 byte header shape.
// Minor version 7 made the contact body sim locators persistent
#define B3_REC_VERSION_MINOR 8
#define B3_REC_VERSION_MINOR 9
// File header, fixed 48 bytes. Contains the registry locator so the player
// can load geometry before replaying any ops.

View file

@ -40,6 +40,7 @@ B3_REC_OP( 0x0C, WorldRebuildStaticTree, RET_NONE, ARG( WORLDID, world ) )
B3_REC_OP( 0x0D, WorldEnableSpeculative, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )
B3_REC_OP( 0x0E, WorldSetRestitutionIterations, RET_NONE, ARG( WORLDID, world ) ARG( I32, iterations ) )
B3_REC_OP( 0x0F, WorldEnableRestitutionPropagation, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )
B3_REC_OP( 0x70, WorldSetIsotropicInertiaTolerance, RET_NONE, ARG( WORLDID, world ) ARG( F32, tolerance ) )
// Body
B3_REC_OP( 0x10, CreateBody, RET_BODYID, ARG( WORLDID, world ) ARG( BODYDEF, def ) )

View file

@ -806,6 +806,12 @@ static void b3RecDispatch_WorldEnableRestitutionPropagation( const b3RecArgs_Wor
b3World_EnableRestitutionPropagation( rdr->replayWorldId, a->flag );
}
static void b3RecDispatch_WorldSetIsotropicInertiaTolerance( const b3RecArgs_WorldSetIsotropicInertiaTolerance* a,
b3RecReader* rdr )
{
b3World_SetIsotropicInertiaTolerance( rdr->replayWorldId, a->tolerance );
}
static void b3RecDispatch_CreateBody( const b3RecArgs_CreateBody* a, b3RecReader* rdr )
{
b3BodyId recId = b3RecR_BODYID( rdr );

View file

@ -582,7 +582,6 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
impulseScale = base->constraintSoftness.impulseScale;
}
//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, b3Add( cdot, bias ) );
b3Vec3 impulse = b3Sub( b3MulSV( -massScale, b ), b3MulSV( impulseScale, joint->linearImpulse ) );

View file

@ -106,25 +106,23 @@ static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* conte
// I * (w2 - w1) + h * cross(w2, I * w2) = 0
// This is all done in local coordinates where the Jacobian is easier to compute.
// This improves the simulation of long skinny bodies.
if ( sim->flags & b3_enableGyroscopicTorque )
{
// Get current rotation.
b3Quat q0 = sim->transform.q;
b3Quat q = b3MulQuat( state->deltaRotation, q0 );
// todo wasteful computation
b3Matrix3 inertiaLocal = b3InvertMatrix( sim->invInertiaLocal );
// Compute local angular velocity
b3Vec3 omega1 = b3InvRotateVector( q, w );
b3Vec3 omega2 = omega1;
// Symmetric inertia tensor: 6 unique entries (column-major)
float i00 = inertiaLocal.cx.x;
float i01 = inertiaLocal.cy.x;
float i02 = inertiaLocal.cz.x;
float i11 = inertiaLocal.cy.y;
float i12 = inertiaLocal.cz.y;
float i22 = inertiaLocal.cz.z;
float i00 = sim->inertia.cx.x;
float i01 = sim->inertia.cy.x;
float i02 = sim->inertia.cz.x;
float i11 = sim->inertia.cy.y;
float i12 = sim->inertia.cz.y;
float i22 = sim->inertia.cz.z;
for ( int gyroIteration = 0; gyroIteration < B3_GYROSCOPIC_ITERATIONS; ++gyroIteration )
{

View file

@ -633,7 +633,6 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi
impulseScale = base->constraintSoftness.impulseScale;
}
//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, b3Add( cdot, bias ) );
b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse );

View file

@ -25,6 +25,8 @@ b3WorldDef b3DefaultWorldDef( void )
// 400 meters per second, faster than the speed of sound
def.maximumLinearSpeed = 400.0f * lengthUnits;
def.isotropicInertiaTolerance = 0.2f;
def.enableSleep = true;
def.enableContinuous = true;
def.internalValue = B3_SECRET_COOKIE;

View file

@ -269,7 +269,6 @@ void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias )
impulseScale = joint->linearSpring.impulseScale;
}
//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, b3Add( cdot, bias ) );
b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse );

View file

@ -33,7 +33,7 @@
// Snapshot image magic 'BNS3' and version
#define B3_SNAP_MAGIC 0x33534E42u
#define B3_SNAP_VERSION 10u // post solve restitution
#define B3_SNAP_VERSION 11u // isotropic inertia tolerance
#define B3_SNAP_FLAG_VALIDATION 0x1u
#define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u
@ -492,6 +492,7 @@ static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world )
b3SnapW_Bytes( buf, &world->restitutionThreshold, sizeof( float ) );
b3SnapW_I32( buf, world->restitutionIterations );
b3SnapW_Bytes( buf, &world->maxLinearSpeed, sizeof( float ) );
b3SnapW_Bytes( buf, &world->isotropicInertiaTolerance, sizeof( float ) );
b3SnapW_Bytes( buf, &world->contactSpeed, sizeof( float ) );
b3SnapW_Bytes( buf, &world->contactHertz, sizeof( float ) );
b3SnapW_Bytes( buf, &world->contactDampingRatio, sizeof( float ) );
@ -519,6 +520,7 @@ static void b3DesWorldConfig( b3SnapReader* r, b3World* world )
b3SnapR_Bytes( r, &world->restitutionThreshold, sizeof( float ) );
world->restitutionIterations = b3SnapR_I32( r );
b3SnapR_Bytes( r, &world->maxLinearSpeed, sizeof( float ) );
b3SnapR_Bytes( r, &world->isotropicInertiaTolerance, sizeof( float ) );
b3SnapR_Bytes( r, &world->contactSpeed, sizeof( float ) );
b3SnapR_Bytes( r, &world->contactHertz, sizeof( float ) );
b3SnapR_Bytes( r, &world->contactDampingRatio, sizeof( float ) );

View file

@ -475,6 +475,174 @@ static int ShapeExtents( void )
return 0;
}
static int CompoundInertiaConsistency( void )
{
b3WorldDef worldDef = b3DefaultWorldDef();
b3WorldId worldId = b3CreateWorld( &worldDef );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ 0.3f, -0.8f, 0.5f } ), 1.3f );
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3BoxHull boxA = b3MakeOffsetBoxHull( 0.5f, 0.25f, 0.5f, (b3Vec3){ 1.0f, 1.0f, 0.0f } );
b3BoxHull boxB = b3MakeOffsetBoxHull( 0.3f, 0.6f, 0.2f, (b3Vec3){ -0.8f, -0.4f, 0.3f } );
b3CreateHullShape( bodyId, &shapeDef, &boxA.base );
b3CreateHullShape( bodyId, &shapeDef, &boxB.base );
b3Body_ApplyMassFromShapes( bodyId );
b3Matrix3 local = b3Body_GetLocalRotationalInertia( bodyId );
ENSURE( b3AbsFloat( local.cy.x ) > 0.1f );
b3Matrix3 r = b3MakeMatrixFromQuat( bodyDef.rotation );
b3Matrix3 world = b3MulMM( b3MulMM( r, local ), b3Transpose( r ) );
b3Matrix3 invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
b3Matrix3 product = b3MulMM( invWorld, world );
ENSURE_SMALL( product.cx.x - 1.0f, 1.0e-4f );
ENSURE_SMALL( product.cy.y - 1.0f, 1.0e-4f );
ENSURE_SMALL( product.cz.z - 1.0f, 1.0e-4f );
ENSURE_SMALL( product.cx.y, 1.0e-4f );
ENSURE_SMALL( product.cx.z, 1.0e-4f );
ENSURE_SMALL( product.cy.x, 1.0e-4f );
ENSURE_SMALL( product.cy.z, 1.0e-4f );
ENSURE_SMALL( product.cz.x, 1.0e-4f );
ENSURE_SMALL( product.cz.y, 1.0e-4f );
b3Quat q2 = b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ -1.0f, 0.2f, 0.4f } ), -2.1f );
b3Body_SetTransform( bodyId, b3Body_GetPosition( bodyId ), q2 );
r = b3MakeMatrixFromQuat( q2 );
world = b3MulMM( b3MulMM( r, local ), b3Transpose( r ) );
invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
product = b3MulMM( invWorld, world );
ENSURE_SMALL( product.cx.x - 1.0f, 1.0e-4f );
ENSURE_SMALL( product.cy.y - 1.0f, 1.0e-4f );
ENSURE_SMALL( product.cz.z - 1.0f, 1.0e-4f );
ENSURE_SMALL( product.cy.x, 1.0e-4f );
ENSURE_SMALL( product.cz.y, 1.0e-4f );
b3DestroyWorld( worldId );
return 0;
}
static b3BodyId CreateSpinningHullBody( b3WorldId worldId, float x, const b3HullData* hull )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position.x = x;
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ 0.3f, -0.8f, 0.5f } ), 1.3f );
bodyDef.angularVelocity = (b3Vec3){ 1.0f, -2.0f, 3.0f };
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3CreateHullShape( bodyId, &shapeDef, hull );
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
// angular velocity is untouched. Anisotropic bodies precess.
static int IsotropicInertiaTolerance( void )
{
b3Vec3 w0 = { 1.0f, -2.0f, 3.0f };
b3HullData* rock = b3CreateRock( 0.5f );
b3HullData* complex = b3CreateComplexHull( 0.5f );
b3BoxHull cube = b3MakeCubeHull( 0.5f );
b3Transform cubeTransform = { b3Vec3_zero, b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ 1.0f, 2.0f, 3.0f } ), 0.7f ) };
b3BoxHull rotatedCube = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, cubeTransform );
b3BoxHull plank = b3MakeBoxHull( 0.5f, 1.0f, 0.25f );
{
b3WorldDef worldDef = b3DefaultWorldDef();
worldDef.gravity = b3Vec3_zero;
b3WorldId worldId = b3CreateWorld( &worldDef );
b3BodyId rockId = CreateSpinningHullBody( worldId, 0.0f, rock );
b3BodyId complexId = CreateSpinningHullBody( worldId, 5.0f, complex );
b3BodyId cubeId = CreateSpinningHullBody( worldId, 10.0f, &cube.base );
b3BodyId rotatedCubeId = CreateSpinningHullBody( worldId, 15.0f, &rotatedCube.base );
b3BodyId plankId = CreateSpinningHullBody( worldId, 20.0f, &plank.base );
ENSURE( HasGyroscopicTorque( rockId ) == false );
ENSURE( HasGyroscopicTorque( complexId ) == false );
ENSURE( HasGyroscopicTorque( cubeId ) == false );
ENSURE( HasGyroscopicTorque( rotatedCubeId ) == false );
ENSURE( HasGyroscopicTorque( plankId ) );
for ( int i = 0; i < 10; ++i )
{
b3World_Step( worldId, 1.0f / 60.0f, 4 );
}
b3Vec3 w = b3Body_GetAngularVelocity( rockId );
ENSURE( w.x == w0.x && w.y == w0.y && w.z == w0.z );
w = b3Body_GetAngularVelocity( complexId );
ENSURE( w.x == w0.x && w.y == w0.y && w.z == w0.z );
w = b3Body_GetAngularVelocity( rotatedCubeId );
ENSURE( w.x == w0.x && w.y == w0.y && w.z == w0.z );
w = b3Body_GetAngularVelocity( plankId );
ENSURE( b3Length( b3Sub( w, w0 ) ) > 0.01f );
b3Body_SetMotionLocks( plankId, (b3MotionLocks){ .angularX = true, .angularY = true, .angularZ = true } );
ENSURE( HasGyroscopicTorque( plankId ) == false );
// Gyroscopic torque stays off until a mass update proves the body anisotropic
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position.x = 25.0f;
bodyDef.angularVelocity = w0;
b3BodyId emptyId = b3CreateBody( worldId, &bodyDef );
ENSURE( HasGyroscopicTorque( emptyId ) == false );
bodyDef.position.x = 30.0f;
b3BodyId deferredId = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.updateBodyMass = false;
b3CreateHullShape( deferredId, &shapeDef, &plank.base );
ENSURE( HasGyroscopicTorque( deferredId ) == false );
b3Body_ApplyMassFromShapes( deferredId );
ENSURE( HasGyroscopicTorque( deferredId ) );
b3DestroyWorld( worldId );
}
{
b3WorldDef worldDef = b3DefaultWorldDef();
worldDef.gravity = b3Vec3_zero;
worldDef.isotropicInertiaTolerance = 0.0f;
b3WorldId worldId = b3CreateWorld( &worldDef );
b3BodyId rockId = CreateSpinningHullBody( worldId, 0.0f, rock );
b3BodyId cubeId = CreateSpinningHullBody( worldId, 10.0f, &cube.base );
ENSURE( HasGyroscopicTorque( rockId ) );
ENSURE( HasGyroscopicTorque( cubeId ) == false );
for ( int i = 0; i < 10; ++i )
{
b3World_Step( worldId, 1.0f / 60.0f, 4 );
}
b3Vec3 w = b3Body_GetAngularVelocity( rockId );
ENSURE( b3Length( b3Sub( w, w0 ) ) > 0.001f );
b3DestroyWorld( worldId );
}
b3DestroyHull( rock );
b3DestroyHull( complex );
return 0;
}
int BodyTest( void )
{
RUN_SUBTEST( FarSingleSphereMass );
@ -486,5 +654,7 @@ int BodyTest( void )
RUN_SUBTEST( SetMassDataZeroMass );
RUN_SUBTEST( SetMassDataConsistentVelocity );
RUN_SUBTEST( ShapeExtents );
RUN_SUBTEST( CompoundInertiaConsistency );
RUN_SUBTEST( IsotropicInertiaTolerance );
return 0;
}

View file

@ -19,27 +19,27 @@
// Double precision accumulates body positions in double, so the settle/sleep step and the
// state hash differ from the float build. Both modes are internally deterministic.
#if defined( BOX3D_DOUBLE_PRECISION )
#define RAGDOLL_SLEEP_STEP 267
#define RAGDOLL_HASH 0x01EB8FDB
#define WAVE_PILE_SLEEP_STEP 333
#define WAVE_PILE_HASH 0x80D5266B
#define RAGDOLL_SLEEP_STEP 263
#define RAGDOLL_HASH 0xA5423F98
#define WAVE_PILE_SLEEP_STEP 272
#define WAVE_PILE_HASH 0xC8E5F3D6
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0x90EE3E7C
#define QUERY_SPAWN_HASH 0x489585E6
#define QUERY_SPAWN_HIT_COUNT 59
#define QUERY_SPAWN_QUERY_HASH 0x5B4429DC
#define MESH_DROP_SLEEP_STEP 218
#define MESH_DROP_HASH 0x10051259
#define MESH_DROP_HASH 0xD378CF04
#else
#define RAGDOLL_SLEEP_STEP 265
#define RAGDOLL_HASH 0xB98B3638
#define WAVE_PILE_SLEEP_STEP 288
#define WAVE_PILE_HASH 0xFC1CD2DC
#define RAGDOLL_SLEEP_STEP 291
#define RAGDOLL_HASH 0xFF4C605B
#define WAVE_PILE_SLEEP_STEP 272
#define WAVE_PILE_HASH 0x266F83B0
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0x58F47348
#define QUERY_SPAWN_HASH 0xF3AD9BA6
#define QUERY_SPAWN_HIT_COUNT 59
#define QUERY_SPAWN_QUERY_HASH 0xE3271F3D
#define MESH_DROP_SLEEP_STEP 218
#define MESH_DROP_HASH 0x983F27EA
#define MESH_DROP_SLEEP_STEP 226
#define MESH_DROP_HASH 0xD2FF069D
#endif
// The goldens above pin exact values for the default four point manifold. A build that

View file

@ -1584,6 +1584,21 @@ static int AllOps( void )
b3World_SetMaximumLinearSpeed( worldId, 100.0f );
b3World_RebuildStaticTree( worldId );
// The tolerance only reaches bodies whose mass is computed afterwards. A free spinning rod made
// after the change loses its gyroscopic torque, so replay diverges if the op goes missing.
b3World_SetIsotropicInertiaTolerance( worldId, 1.0f );
{
b3BodyDef rodDef = b3DefaultBodyDef();
rodDef.type = b3_dynamicBody;
rodDef.position = (b3Pos){ 40.0f, 10.0f, 0.0f };
rodDef.gravityScale = 0.0f;
rodDef.angularVelocity = (b3Vec3){ 1.0f, 5.0f, 0.5f };
b3BodyId rodId = b3CreateBody( worldId, &rodDef );
b3BoxHull rod = b3MakeBoxHull( 0.1f, 1.0f, 0.1f );
b3ShapeDef rodShapeDef = b3DefaultShapeDef();
b3CreateHullShape( rodId, &rodShapeDef, &rod.base );
}
b3ExplosionDef explosion = b3DefaultExplosionDef();
explosion.position = (b3Pos){ 0.0f, 5.0f, 0.0f };
explosion.radius = 3.0f;

View file

@ -315,6 +315,14 @@ int TestWorldCoverage( void )
value = b3World_GetMaximumLinearSpeed( worldId );
ENSURE( value == 10.0f );
b3World_SetIsotropicInertiaTolerance( worldId, 0.5f );
value = b3World_GetIsotropicInertiaTolerance( worldId );
ENSURE( value == 0.5f );
b3World_SetIsotropicInertiaTolerance( worldId, 2.0f );
value = b3World_GetIsotropicInertiaTolerance( worldId );
ENSURE( value == 1.0f );
b3World_EnableWarmStarting( worldId, true );
flag = b3World_IsWarmStartingEnabled( worldId );
ENSURE( flag == true );