isotropic test

This commit is contained in:
Erin Catto 2026-10-15 12:59:35 -07:00
commit c1779e6ba5
10 changed files with 202 additions and 7 deletions

View file

@ -173,6 +173,11 @@ typedef struct b3WorldDef
/// Maximum linear speed. Usually meters per second.
float maximumLinearSpeed;
/// Use this to disable gyroscopic torque on sphere-like objects automatically.
/// A value of 0.2 means up to 20% non-isotropic inertia values are treated as
/// isotropic.
float isotropicInertiaTolerance;
/// Optional mixing callback for friction. The default uses sqrt(frictionA * frictionB).
b3FrictionCallback* frictionCallback;

View file

@ -892,12 +892,37 @@ 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 )
{
b3Matrix3 inertia = body->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;
float deviation = dx * dx + dy * dy + dz * dz + 2.0f * offDiagonal;
float tolerance = world->isotropicInertiaTolerance;
if ( 2.0f * deviation <= tolerance * tolerance * mean * mean )
{
body->flags |= b3_isotropicInertia;
}
else
{
body->flags &= ~b3_isotropicInertia;
}
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_isotropicInertia;
b3SyncBodyFlags( world, body );
// Compute mass data from shapes. Each shape has its own density.
@ -1052,6 +1077,8 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
bodySim->invInertiaLocal = b3Mat3_zero;
bodySim->invInertiaWorld = b3Mat3_zero;
}
b3UpdateIsotropicInertia( world, body );
}
b3Pos b3Body_GetPosition( b3BodyId bodyId )
@ -1917,6 +1944,8 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
bodySim->invInertiaWorld = b3Mat3_zero;
}
b3UpdateIsotropicInertia( world, body );
// Update extents using supplied mass center.
bodySim->minExtent = B3_HUGE;
bodySim->maxExtent = b3Vec3_zero;

View file

@ -64,6 +64,8 @@ enum b3BodyFlags
// data still hasn't been set.
b3_dirtyMass = 0x00008000,
b3_isotropicInertia = 0x00010000,
// All lock flags
b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,

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 = b3MaxFloat( def->isotropicInertiaTolerance, 0.0f );
world->contactSpeed = def->contactSpeed;
world->contactHertz = def->contactHertz;
world->contactDampingRatio = def->contactDampingRatio;

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

@ -106,6 +106,7 @@ 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_isotropicInertia ) == 0 )
{
// Get current rotation.
b3Quat q0 = sim->transform.q;

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

@ -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,156 @@ 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 IsIsotropic( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
return ( body->flags & b3_isotropicInertia ) != 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( IsIsotropic( rockId ) );
ENSURE( IsIsotropic( complexId ) );
ENSURE( IsIsotropic( cubeId ) );
ENSURE( IsIsotropic( rotatedCubeId ) );
ENSURE( IsIsotropic( plankId ) == false );
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( IsIsotropic( plankId ) );
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( IsIsotropic( rockId ) == false );
ENSURE( IsIsotropic( cubeId ) );
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 +636,7 @@ int BodyTest( void )
RUN_SUBTEST( SetMassDataZeroMass );
RUN_SUBTEST( SetMassDataConsistentVelocity );
RUN_SUBTEST( ShapeExtents );
RUN_SUBTEST( CompoundInertiaConsistency );
RUN_SUBTEST( IsotropicInertiaTolerance );
return 0;
}

View file

@ -21,10 +21,10 @@
#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 WAVE_PILE_SLEEP_STEP 272
#define WAVE_PILE_HASH 0xF7EF20F7
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0x90EE3E7C
#define QUERY_SPAWN_HASH 0xCA68A74D
#define QUERY_SPAWN_HIT_COUNT 59
#define QUERY_SPAWN_QUERY_HASH 0x5B4429DC
#define MESH_DROP_SLEEP_STEP 218
@ -32,10 +32,10 @@
#else
#define RAGDOLL_SLEEP_STEP 265
#define RAGDOLL_HASH 0xB98B3638
#define WAVE_PILE_SLEEP_STEP 288
#define WAVE_PILE_HASH 0xFC1CD2DC
#define WAVE_PILE_SLEEP_STEP 275
#define WAVE_PILE_HASH 0xA6B9D38B
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0x58F47348
#define QUERY_SPAWN_HASH 0x2DA37C5A
#define QUERY_SPAWN_HIT_COUNT 59
#define QUERY_SPAWN_QUERY_HASH 0xE3271F3D
#define MESH_DROP_SLEEP_STEP 218