From c1779e6ba553a0564f01e2a9c5fca38cf5f29fc8 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Thu, 15 Oct 2026 12:59:35 -0700 Subject: [PATCH] isotropic test --- include/box3d/types.h | 5 ++ src/body.c | 29 ++++++++ src/body.h | 2 + src/physics_world.c | 1 + src/physics_world.h | 1 + src/solver.c | 1 + src/types.c | 2 + src/world_snapshot.c | 4 +- test/test_body.c | 152 ++++++++++++++++++++++++++++++++++++++++ test/test_determinism.c | 12 ++-- 10 files changed, 202 insertions(+), 7 deletions(-) diff --git a/include/box3d/types.h b/include/box3d/types.h index 57e5e75..cec244f 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -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; diff --git a/src/body.c b/src/body.c index 4b96393..3ab53e7 100644 --- a/src/body.c +++ b/src/body.c @@ -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; diff --git a/src/body.h b/src/body.h index 3e1c6d4..66a1843 100644 --- a/src/body.h +++ b/src/body.h @@ -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, diff --git a/src/physics_world.c b/src/physics_world.c index c48253d..00aa8bb 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -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; diff --git a/src/physics_world.h b/src/physics_world.h index af419de..1436ee4 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -238,6 +238,7 @@ typedef struct b3World int restitutionIterations; bool enableRestitutionPropagation; float maxLinearSpeed; + float isotropicInertiaTolerance; float contactSpeed; float contactHertz; float contactDampingRatio; diff --git a/src/solver.c b/src/solver.c index 17367bb..4fb8601 100644 --- a/src/solver.c +++ b/src/solver.c @@ -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; diff --git a/src/types.c b/src/types.c index 12f4c09..9caab0a 100644 --- a/src/types.c +++ b/src/types.c @@ -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; diff --git a/src/world_snapshot.c b/src/world_snapshot.c index 8cf0d40..e763fde 100644 --- a/src/world_snapshot.c +++ b/src/world_snapshot.c @@ -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 ) ); diff --git a/test/test_body.c b/test/test_body.c index 7200f61..cb3ba14 100644 --- a/test/test_body.c +++ b/test/test_body.c @@ -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; } diff --git a/test/test_determinism.c b/test/test_determinism.c index 966c4ba..e0f8e68 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -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