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:
parent
1bec63c9ee
commit
0f6fdad541
14 changed files with 94 additions and 30 deletions
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 ---
|
||||
|
|
|
|||
|
|
@ -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 ) );
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 ) );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
13
src/body.c
13
src/body.c
|
|
@ -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 );
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue