From efca0c184d40fd6a82016983f1601c4d23965867 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Thu, 15 Oct 2026 23:30:56 -0700 Subject: [PATCH] Gyro tolerance (#191) --- include/box3d/box3d.h | 7 ++ include/box3d/types.h | 4 + samples/sample_bodies.cpp | 101 ++++++++++++++++++++++ src/body.c | 69 ++++++++++++---- src/body.h | 10 ++- src/contact.c | 3 +- src/math_internal.h | 10 --- src/motor_joint.c | 3 +- src/physics_world.c | 22 +++++ src/physics_world.h | 1 + src/recording.h | 2 +- src/recording_ops.inl | 1 + src/replay.c | 6 ++ src/revolute_joint.c | 1 - src/solver.c | 16 ++-- src/spherical_joint.c | 1 - src/types.c | 2 + src/weld_joint.c | 1 - src/world_snapshot.c | 4 +- test/test_body.c | 170 ++++++++++++++++++++++++++++++++++++++ test/test_determinism.c | 26 +++--- test/test_recording.c | 15 ++++ test/test_world.c | 8 ++ 23 files changed, 420 insertions(+), 63 deletions(-) diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index 65a9d47..54d0baf 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -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 ); diff --git a/include/box3d/types.h b/include/box3d/types.h index 57e5e75..2cbbbeb 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -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; diff --git a/samples/sample_bodies.cpp b/samples/sample_bodies.cpp index b24f65e..cc58786 100644 --- a/samples/sample_bodies.cpp +++ b/samples/sample_bodies.cpp @@ -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: diff --git a/src/body.c b/src/body.c index 4b96393..6d67343 100644 --- a/src/body.c +++ b/src/body.c @@ -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 ) diff --git a/src/body.h b/src/body.h index 3e1c6d4..b03063d 100644 --- a/src/body.h +++ b/src/body.h @@ -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; diff --git a/src/contact.c b/src/contact.c index 79436d9..d200301 100644 --- a/src/contact.c +++ b/src/contact.c @@ -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; diff --git a/src/math_internal.h b/src/math_internal.h index d975c65..b272bdc 100644 --- a/src/math_internal.h +++ b/src/math_internal.h @@ -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 ); diff --git a/src/motor_joint.c b/src/motor_joint.c index dc8913c..9487d0c 100644 --- a/src/motor_joint.c +++ b/src/motor_joint.c @@ -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 ); diff --git a/src/physics_world.c b/src/physics_world.c index c48253d..192cfdb 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 = 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 ); 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/recording.h b/src/recording.h index 0bc3fcf..6f9df4d 100644 --- a/src/recording.h +++ b/src/recording.h @@ -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. diff --git a/src/recording_ops.inl b/src/recording_ops.inl index d2c992b..ed4b7d6 100644 --- a/src/recording_ops.inl +++ b/src/recording_ops.inl @@ -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 ) ) diff --git a/src/replay.c b/src/replay.c index 26f20a0..7a46d73 100644 --- a/src/replay.c +++ b/src/replay.c @@ -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 ); diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 0e568f1..5f8b6da 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -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 ) ); diff --git a/src/solver.c b/src/solver.c index 17367bb..f1697bd 100644 --- a/src/solver.c +++ b/src/solver.c @@ -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 ) { diff --git a/src/spherical_joint.c b/src/spherical_joint.c index 537cc8a..cc9692c 100644 --- a/src/spherical_joint.c +++ b/src/spherical_joint.c @@ -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 ); 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/weld_joint.c b/src/weld_joint.c index 075bbdf..5a379a7 100644 --- a/src/weld_joint.c +++ b/src/weld_joint.c @@ -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 ); 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..22337c7 100644 --- a/test/test_body.c +++ b/test/test_body.c @@ -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; } diff --git a/test/test_determinism.c b/test/test_determinism.c index 966c4ba..f2bd7e6 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -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 diff --git a/test/test_recording.c b/test/test_recording.c index c01fe89..12f7316 100644 --- a/test/test_recording.c +++ b/test/test_recording.c @@ -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; diff --git a/test/test_world.c b/test/test_world.c index 1078609..c9022ef 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -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 );