diff --git a/docs/large_worlds.md b/docs/large_worlds.md index 25e5ba2..6641514 100644 --- a/docs/large_worlds.md +++ b/docs/large_worlds.md @@ -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. diff --git a/docs/simulation.md b/docs/simulation.md index 9a778e7..89a3fad 100644 --- a/docs/simulation.md +++ b/docs/simulation.md @@ -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 diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index 3baff6a..62c61be 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -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 diff --git a/samples/sample.cpp b/samples/sample.cpp index 6ef0bc2..41f66a7 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -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 ); diff --git a/samples/sample_bodies.cpp b/samples/sample_bodies.cpp index 115fa49..08b9797 100644 --- a/samples/sample_bodies.cpp +++ b/samples/sample_bodies.cpp @@ -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 ); } } diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index 0aa5477..17af0f2 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -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 --- diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 9592877..317c8bc 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -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 ) ); diff --git a/samples/sample_events.cpp b/samples/sample_events.cpp index 85b4cd2..ace7823 100644 --- a/samples/sample_events.cpp +++ b/samples/sample_events.cpp @@ -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; } diff --git a/samples/sample_joint.cpp b/samples/sample_joint.cpp index ac5d2fd..a9b5223 100644 --- a/samples/sample_joint.cpp +++ b/samples/sample_joint.cpp @@ -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 ); diff --git a/samples/sample_replay.cpp b/samples/sample_replay.cpp index 42671de..9ef2d0a 100644 --- a/samples/sample_replay.cpp +++ b/samples/sample_replay.cpp @@ -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 ) ); } } } diff --git a/samples/sample_world.cpp b/samples/sample_world.cpp index 241a1fc..1f8d142 100644 --- a/samples/sample_world.cpp +++ b/samples/sample_world.cpp @@ -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 ); diff --git a/shared/benchmarks.c b/shared/benchmarks.c index e2ce39c..64583cb 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -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 ); diff --git a/src/body.c b/src/body.c index 94ef43d..fab7a6b 100644 --- a/src/body.c +++ b/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 ); diff --git a/test/test_body.c b/test/test_body.c index c93ab47..98a3408 100644 --- a/test/test_body.c +++ b/test/test_body.c @@ -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; }