Set mass data consistent velocity (#45)

- Rename b3Body_GetWorldCenterOfMass to b3Body_GetWorldCenter.
- Rename b3Body_GetLocalCenterOfMass to b3Body_GetLocalCenter.
- Fixed b3Body_SetMassData to correct velocity when local center moves.
This commit is contained in:
Erin Catto 2026-07-04 11:22:29 -07:00 • committed by GitHub
commit 0f6fdad541
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
14 changed files with 94 additions and 30 deletions

View file

@ -46,7 +46,7 @@ quaternions in both modes, so the trig and the cross-platform determinism proper
The public API uses these types wherever it accepts or returns a world position: `b3BodyDef.position`,
`b3Body_GetPosition` / `b3Body_GetTransform`, `b3Body_SetTransform`, `b3Body_GetWorldPoint` /
`b3Body_GetLocalPoint`, `b3Body_GetWorldCenterOfMass`, the explosion and ray-cast origins, contact
`b3Body_GetLocalPoint`, `b3Body_GetWorldCenter`, the explosion and ray-cast origins, contact
and ray-cast result points, and the body move event. With double precision off these are all the
float types they have always been, so existing code compiles unchanged.

View file

@ -490,11 +490,12 @@ The body's mass data is available through the following functions:
```c
float mass = b3Body_GetMass(myBodyId);
b3Matrix3 inertia = b3Body_GetLocalRotationalInertia(myBodyId);
b3Vec3 localCenter = b3Body_GetLocalCenterOfMass(myBodyId);
b3Vec3 localCenter = b3Body_GetLocalCenter(myBodyId);
b3MassData massData = b3Body_GetMassData(myBodyId);
```
### State Information
There are many aspects to the body's state. You can access this state
data through the following functions:
@ -537,8 +538,8 @@ body that is a box. The body origin might be a corner of the box,
while the center of mass is located at the center of the box.
```c
b3Vec3 worldCenter = b3Body_GetWorldCenterOfMass(myBodyId);
b3Vec3 localCenter = b3Body_GetLocalCenterOfMass(myBodyId);
b3Pos worldCenter = b3Body_GetWorldCenter(myBodyId);
b3Vec3 localCenter = b3Body_GetLocalCenter(myBodyId);
```
You can access the linear and angular velocity. The linear velocity is

View file

@ -636,10 +636,10 @@ B3_API float b3Body_GetInverseMass( b3BodyId bodyId );
B3_API b3Matrix3 b3Body_GetWorldInverseRotationalInertia( b3BodyId bodyId );
/// Get the center of mass position of the body in local space
B3_API b3Vec3 b3Body_GetLocalCenterOfMass( b3BodyId bodyId );
B3_API b3Vec3 b3Body_GetLocalCenter( b3BodyId bodyId );
/// Get the center of mass position of the body in world space
B3_API b3Pos b3Body_GetWorldCenterOfMass( b3BodyId bodyId );
B3_API b3Pos b3Body_GetWorldCenter( b3BodyId bodyId );
/// Override the body's mass properties. Normally this is computed automatically using the
/// shape geometry and density. This information is lost if a shape is added or removed or if the

View file

@ -2292,7 +2292,7 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
float invMassB = b3Body_GetInverseMass( bodyId );
b3Matrix3 invIB = b3Body_GetWorldInverseRotationalInertia( bodyId );
b3Pos pB = b3Body_GetWorldCenterOfMass( bodyId );
b3Pos pB = b3Body_GetWorldCenter( bodyId );
b3Vec3 rB = b3SubPos( point, pB );
b3Vec3 rnB = b3Cross( rB, normal );

View file

@ -334,23 +334,37 @@ public:
bodyDef.type = b3_dynamicBody;
bodyDef.position = { 0.0f, 2.0f, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, -0.5f * B3_PI );
bodyDef.angularVelocity = { 0.01f, 0.01f, 10.0f };
bodyDef.gravityScale = 0.0f;
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
m_bodyId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.updateBodyMass = false;
b3HullData* cylinder = b3CreateCylinder( 0.6f, 0.15f, 0.0f, 32 );
b3BoxHull box = b3MakeBoxHull( 1.0f, 0.05f, 0.1f );
b3CreateHullShape( bodyId, &shapeDef, cylinder );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
b3CreateHullShape( m_bodyId, &shapeDef, cylinder );
b3CreateHullShape( m_bodyId, &shapeDef, &box.base );
b3Body_ApplyMassFromShapes( m_bodyId );
// Set the angular velocity after creating the shapes and the local center of mass is fixed.
b3Body_SetAngularVelocity( m_bodyId, { 0.01f, 0.01f, 10.0f } );
b3DestroyHull( cylinder );
}
void Step() override
{
Sample::Step();
b3Pos c = b3Body_GetWorldCenter( m_bodyId );
DrawTextLine( "center %.3g %.3g %.3g", c.x, c.y, c.z );
}
static Sample* Create( SampleContext* sampleContext )
{
return new GyroscopicTorque( sampleContext );
}
b3BodyId m_bodyId;
};
static int sampleGyroscopicTorque = RegisterSample( "Bodies", "Gyroscopic Torque", GyroscopicTorque::Create );
@ -631,7 +645,7 @@ public:
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transform.p = m_baseTranslation + b3SubPos(origin, m_origin);
m_transform.p = m_baseTranslation + b3SubPos( origin, m_origin );
}
}

View file

@ -1249,7 +1249,7 @@ struct RigidbodyCharacter
// 3. TryStep — 4-phase step-up
m_didStep = TryStep( m_stepUpHeight );
m_massCenterWorld = b3Body_GetWorldCenterOfMass( m_bodyId );
m_massCenterWorld = b3Body_GetWorldCenter( m_bodyId );
}
// --- PostStep: RestoreStep + Reground + CategorizeGround ---

View file

@ -666,7 +666,7 @@ public:
continue;
}
b3Pos massCenter = b3Body_GetWorldCenterOfMass( bodyId );
b3Pos massCenter = b3Body_GetWorldCenter( bodyId );
if ( massCenter.y < -2.0f )
{
IndexPair pair = ConvertToPair( b3Body_GetUserData( bodyId ) );

View file

@ -160,7 +160,7 @@ public:
{
b3Body_ApplyMassFromShapes( bodyId );
b3Pos center = b3Body_GetWorldCenterOfMass( bodyId );
b3Pos center = b3Body_GetWorldCenter( bodyId );
b3Vec3 omega = { 0.0f, 0.0f, -1.0f * velocityScale };
b3Vec3 v = b3Cross( omega, b3SubPos( center, b3OffsetPos( b3Pos_zero, origin ) ) );
b3Body_SetAngularVelocity( bodyId, omega );
@ -272,7 +272,7 @@ public:
b3BoxHull dynamicBox = b3MakeTransformedBoxHull( 0.5f, 10.0f, 0.5f, { { 0.0f, 10.0f, 0.0f }, b3Quat_identity } );
b3CreateHullShape( m_bodyId, &shapeDef, &dynamicBox.base );
b3Pos center = b3Body_GetWorldCenterOfMass( m_bodyId );
b3Pos center = b3Body_GetWorldCenter( m_bodyId );
b3Vec3 r = b3SubPos( pivot, center );
float rr = b3LengthSquared( r );
@ -635,7 +635,7 @@ public:
if ( B3_IS_NON_NULL( m_contactId ) && b3Contact_IsValid( m_contactId ) )
{
b3ContactData data = b3Contact_GetData( m_contactId );
b3Pos centerOfMass = b3Body_GetWorldCenterOfMass( b3Shape_GetBody( data.shapeIdA ) );
b3Pos centerOfMass = b3Body_GetWorldCenter( b3Shape_GetBody( data.shapeIdA ) );
for ( int i = 0; i < data.manifoldCount; ++i )
{
@ -822,7 +822,7 @@ public:
{
b3BodyId sensorBodyId = b3Shape_GetBody( sensorShapeId );
b3WorldTransform t = b3Body_GetTransform( sensorBodyId );
t.p = b3Body_GetWorldCenterOfMass( sensorBodyId );
t.p = b3Body_GetWorldCenter( sensorBodyId );
m_transforms[m_transformCount] = t;
m_transformCount += 1;
}

View file

@ -1161,7 +1161,7 @@ public:
b3Vec3 linearVelocity = b3Body_GetLinearVelocity( m_bodyId );
float kineticEnergy = 0.5f * b3Dot( angularVelocity, b3MulMV( massData.inertia, angularVelocity ) );
kineticEnergy += 0.5f * massData.mass * b3Dot( linearVelocity, linearVelocity );
b3Pos center = b3Body_GetWorldCenterOfMass( m_bodyId );
b3Pos center = b3Body_GetWorldCenter( m_bodyId );
b3Vec3 gravity = b3World_GetGravity( m_worldId );
float potentialEnergy = -massData.mass * center.y * gravity.y;
DrawTextLine( "kinetic energy = %g", kineticEnergy );

View file

@ -938,7 +938,7 @@ public:
int count = b3Body_GetContactData( body, contacts, capacity );
for ( int i = 0; i < count; ++i )
{
b3Pos originA = b3Body_GetWorldCenterOfMass( b3Shape_GetBody( contacts[i].shapeIdA ) );
b3Pos originA = b3Body_GetWorldCenter( b3Shape_GetBody( contacts[i].shapeIdA ) );
for ( int m = 0; m < contacts[i].manifoldCount; ++m )
{
const b3Manifold* manifold = &contacts[i].manifolds[m];
@ -965,7 +965,7 @@ public:
}
b3BodyId body = b3Shape_GetBody( shape );
DrawAxes( b3Body_GetTransform( body ), 0.5f );
DrawPoint( b3Body_GetWorldCenterOfMass( body ), 8.0f, MakeColor( b3_colorYellow ) );
DrawPoint( b3Body_GetWorldCenter( body ), 8.0f, MakeColor( b3_colorYellow ) );
DrawBodyContacts( body );
}
else if ( m_selKind == SelBody )
@ -976,7 +976,7 @@ public:
return;
}
DrawAxes( b3Body_GetTransform( body ), 0.5f );
DrawPoint( b3Body_GetWorldCenterOfMass( body ), 8.0f, MakeColor( b3_colorYellow ) );
DrawPoint( b3Body_GetWorldCenter( body ), 8.0f, MakeColor( b3_colorYellow ) );
DrawBodyContacts( body );
}
else if ( m_selKind == SelJoint )
@ -990,11 +990,11 @@ public:
b3BodyId b = b3Joint_GetBodyB( joint );
if ( b3Body_IsValid( a ) )
{
DrawPoint( b3Body_GetWorldCenterOfMass( a ), 8.0f, MakeColor( b3_colorMagenta ) );
DrawPoint( b3Body_GetWorldCenter( a ), 8.0f, MakeColor( b3_colorMagenta ) );
}
if ( b3Body_IsValid( b ) )
{
DrawPoint( b3Body_GetWorldCenterOfMass( b ), 8.0f, MakeColor( b3_colorMagenta ) );
DrawPoint( b3Body_GetWorldCenter( b ), 8.0f, MakeColor( b3_colorMagenta ) );
}
}
}

View file

@ -101,7 +101,7 @@ public:
// Height of the top box above the ground, measured in the offset's own frame. This holds
// steady at any offset under double precision and drifts once float runs out of resolution.
b3Vec3 top = b3SubPos( b3Body_GetWorldCenterOfMass( m_topBodyId ), m_base );
b3Vec3 top = b3SubPos( b3Body_GetWorldCenter( m_topBodyId ), m_base );
DrawTextLine( "double precision: %s", b3IsDoublePrecision() ? "ON" : "OFF" );
DrawTextLine( "world offset: %.1f km", m_offsetKilometers );

View file

@ -749,7 +749,7 @@ static void CreateTrees( b3WorldId worldId, int scale )
float velocityScale = 0.5f + ( 0.5f * bodyIndex ) / bodyCount;
b3Body_ApplyMassFromShapes( bodyId );
b3Pos center = b3Body_GetWorldCenterOfMass( bodyId );
b3Pos center = b3Body_GetWorldCenter( bodyId );
b3Vec3 omega = { 0.0f, 0.0f, velocityScale * angularVelocity };
b3Vec3 v = b3Cross( omega, b3SubPos( center, bodyDef.position ) );
b3Body_SetAngularVelocity( bodyId, omega );

View file

@ -1800,7 +1800,7 @@ b3Matrix3 b3Body_GetWorldInverseRotationalInertia( b3BodyId bodyId )
return sim->invInertiaWorld;
}
b3Vec3 b3Body_GetLocalCenterOfMass( b3BodyId bodyId )
b3Vec3 b3Body_GetLocalCenter( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
@ -1808,7 +1808,7 @@ b3Vec3 b3Body_GetLocalCenterOfMass( b3BodyId bodyId )
return bodySim->localCenter;
}
b3Pos b3Body_GetWorldCenterOfMass( b3BodyId bodyId )
b3Pos b3Body_GetWorldCenter( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
@ -1841,11 +1841,20 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
body->inertia = massData.inertia;
bodySim->localCenter = massData.center;
b3Pos oldCenter = bodySim->center;
b3Pos center = b3TransformWorldPoint( bodySim->transform, massData.center );
bodySim->center = center;
bodySim->center0 = center;
bodySim->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f;
// Update center of mass velocity
b3BodyState* state = b3GetBodyState( world, body );
if ( state != NULL )
{
b3Vec3 deltaLinear = b3Cross( state->angularVelocity, b3SubPos( bodySim->center, oldCenter ) );
state->linearVelocity = b3Add( state->linearVelocity, deltaLinear );
}
float det = b3Det( body->inertia );
B3_ASSERT( det >= 0.0f );

View file

@ -208,7 +208,7 @@ static int SetMassDataRoundTrip( void )
ENSURE_SMALL( md.inertia.cy.y - 4.0f, 1e-6f );
ENSURE_SMALL( md.inertia.cz.z - 8.0f, 1e-6f );
b3Vec3 localCenter = b3Body_GetLocalCenterOfMass( bodyId );
b3Vec3 localCenter = b3Body_GetLocalCenter( bodyId );
ENSURE_SMALL( localCenter.x - center.x, 1e-6f );
ENSURE_SMALL( localCenter.y - center.y, 1e-6f );
ENSURE_SMALL( localCenter.z - center.z, 1e-6f );
@ -231,7 +231,7 @@ static int SetMassDataRoundTrip( void )
ENSURE_SMALL( invWorld.cy.z, 1e-5f );
// World center of mass is the body origin plus the local center under identity rotation.
b3Pos worldCenter = b3Body_GetWorldCenterOfMass( bodyId );
b3Pos worldCenter = b3Body_GetWorldCenter( bodyId );
ENSURE_SMALL( worldCenter.x - ( 5.0f + center.x ), 1e-5f );
ENSURE_SMALL( worldCenter.y - ( -3.0f + center.y ), 1e-5f );
ENSURE_SMALL( worldCenter.z - ( 2.0f + center.z ), 1e-5f );
@ -346,6 +346,45 @@ static int SetMassDataZeroMass( void )
return 0;
}
// The stored linear velocity tracks the center of mass. Moving the center picks a different material
// point, so a spinning body must have its velocity re-referenced by omega x (newCenter - oldCenter),
// otherwise the mass edit silently injects or drains kinetic energy. Under identity rotation the world
// shift equals the supplied local center, keeping the expected values exact in float.
static int SetMassDataConsistentVelocity( void )
{
b3WorldDef worldDef = b3DefaultWorldDef();
b3WorldId worldId = b3CreateWorld( &worldDef );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = (b3Pos){ 7.0f, 1.0f, -4.0f };
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
// Spin about the origin center, then shift the center of mass off the origin.
b3Vec3 omega = { 1.0f, 2.0f, 4.0f };
b3Body_SetLinearVelocity( bodyId, (b3Vec3){ 1.0f, -2.0f, 3.0f } );
b3Body_SetAngularVelocity( bodyId, omega );
b3Vec3 center = { 0.5f, 0.25f, 0.125f };
b3MassData massData = { 3.0f, center, kDiagInertia };
b3Body_SetMassData( bodyId, massData );
// omega x center = ( 2*0.125 - 4*0.25, 4*0.5 - 1*0.125, 1*0.25 - 2*0.5 ) = ( -0.75, 1.875, -0.75 )
b3Vec3 v = b3Body_GetLinearVelocity( bodyId );
ENSURE_SMALL( v.x - ( 1.0f - 0.75f ), 1e-6f );
ENSURE_SMALL( v.y - ( -2.0f + 1.875f ), 1e-6f );
ENSURE_SMALL( v.z - ( 3.0f - 0.75f ), 1e-6f );
// Only the reference point moved, the angular velocity is untouched.
b3Vec3 w = b3Body_GetAngularVelocity( bodyId );
ENSURE_SMALL( w.x - omega.x, 1e-6f );
ENSURE_SMALL( w.y - omega.y, 1e-6f );
ENSURE_SMALL( w.z - omega.z, 1e-6f );
b3DestroyWorld( worldId );
return 0;
}
int BodyTest( void )
{
RUN_SUBTEST( FarSingleSphereMass );
@ -355,5 +394,6 @@ int BodyTest( void )
RUN_SUBTEST( SetMassDataWorldInertiaRotated );
RUN_SUBTEST( SetMassDataFixedRotation );
RUN_SUBTEST( SetMassDataZeroMass );
RUN_SUBTEST( SetMassDataConsistentVelocity );
return 0;
}