Rotation locking: apply filter only to the world inertia properties to fix the multi-collider case.

24 files changed, 229 insertions, 243 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 20d04b0..9279e4f 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -31,10 +31,10 @@ bitflags::bitflags! {
     #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
     /// Flags affecting the behavior of the constraints solver for a given contact manifold.
     pub(crate) struct RigidBodyFlags: u8 {
-        const IGNORE_COLLIDER_MASS              = 1 << 0;
-        const IGNORE_COLLIDER_ANGULAR_INERTIA_X = 1 << 1;
-        const IGNORE_COLLIDER_ANGULAR_INERTIA_Y = 1 << 2;
-        const IGNORE_COLLIDER_ANGULAR_INERTIA_Z = 1 << 3;
+        const TRANSLATION_LOCKED = 1 << 0;
+        const ROTATION_LOCKED_X = 1 << 1;
+        const ROTATION_LOCKED_Y = 1 << 2;
+        const ROTATION_LOCKED_Z = 1 << 3;
     }
 }
 
@@ -62,8 +62,9 @@ pub struct RigidBody {
     pub(crate) mass_properties: MassProperties,
     /// The world-space center of mass of the rigid-body.
     pub world_com: Point<Real>,
+    pub effective_inv_mass: Real,
     /// The square-root of the inverse angular inertia tensor of the rigid-body.
-    pub world_inv_inertia_sqrt: AngularInertia<Real>,
+    pub effective_world_inv_inertia_sqrt: AngularInertia<Real>,
     /// The linear velocity of the rigid-body.
     pub(crate) linvel: Vector<Real>,
     /// The angular velocity of the rigid-body.
@@ -98,7 +99,8 @@ impl RigidBody {
             predicted_position: Isometry::identity(),
             mass_properties: MassProperties::zero(),
             world_com: Point::origin(),
-            world_inv_inertia_sqrt: AngularInertia::zero(),
+            effective_inv_mass: 0.0,
+            effective_world_inv_inertia_sqrt: AngularInertia::zero(),
             linvel: Vector::zeros(),
             angvel: na::zero(),
             linacc: Vector::zeros(),
@@ -130,14 +132,13 @@ impl RigidBody {
     }
 
     pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector<Real>) {
-        if self.mass_properties.inv_mass != 0.0 {
+        if self.effective_inv_mass != 0.0 {
             self.linvel += (gravity * self.gravity_scale + self.linacc) * dt;
-            self.angvel += self.angacc * dt;
-
-            // Reset the accelerations.
             self.linacc = na::zero();
-            self.angacc = na::zero();
         }
+
+        self.angvel += self.angacc * dt;
+        self.angacc = na::zero();
     }
 
     /// The mass properties of this rigid-body.
@@ -227,40 +228,10 @@ impl RigidBody {
             .mass_properties()
             .transform_by(coll.position_wrt_parent());
         self.colliders.push(handle);
-        self.mass_properties += Self::filter_collider_mass_props(mass_properties, self.flags);
+        self.mass_properties += mass_properties;
         self.update_world_mass_properties();
     }
 
-    fn filter_collider_mass_props(
-        mut props: MassProperties,
-        flags: RigidBodyFlags,
-    ) -> MassProperties {
-        if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_MASS) {
-            props.inv_mass = 0.0;
-        }
-
-        #[cfg(feature = "dim2")]
-        {
-            if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) {
-                props.inv_principal_inertia_sqrt = 0.0;
-            }
-        }
-        #[cfg(feature = "dim3")]
-        {
-            if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X) {
-                props.inv_principal_inertia_sqrt.x = 0.0;
-            }
-            if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y) {
-                props.inv_principal_inertia_sqrt.y = 0.0;
-            }
-            if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) {
-                props.inv_principal_inertia_sqrt.z = 0.0;
-            }
-        }
-
-        props
-    }
-
     pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
         for handle in &self.colliders {
             let collider = &mut colliders[*handle];
@@ -277,7 +248,7 @@ impl RigidBody {
             let mass_properties = coll
                 .mass_properties()
                 .transform_by(coll.position_wrt_parent());
-            self.mass_properties -= Self::filter_collider_mass_props(mass_properties, self.flags);
+            self.mass_properties -= mass_properties;
             self.update_world_mass_properties();
         }
     }
@@ -458,9 +429,41 @@ impl RigidBody {
 
     pub(crate) fn update_world_mass_properties(&mut self) {
         self.world_com = self.mass_properties.world_com(&self.position);
-        self.world_inv_inertia_sqrt = self
+        self.effective_inv_mass = self.mass_properties.inv_mass;
+        self.effective_world_inv_inertia_sqrt = self
             .mass_properties
             .world_inv_inertia_sqrt(&self.position.rotation);
+
+        // Take into account translation/rotation locking.
+        if self.flags.contains(RigidBodyFlags::TRANSLATION_LOCKED) {
+            self.effective_inv_mass = 0.0;
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) {
+                self.effective_world_inv_inertia_sqrt = 0.0;
+            }
+        }
+        #[cfg(feature = "dim3")]
+        {
+            if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_X) {
+                self.effective_world_inv_inertia_sqrt.m11 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m12 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m13 = 0.0;
+            }
+
+            if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Y) {
+                self.effective_world_inv_inertia_sqrt.m22 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m12 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m23 = 0.0;
+            }
+            if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) {
+                self.effective_world_inv_inertia_sqrt.m33 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m13 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m23 = 0.0;
+            }
+        }
     }
 
     /*
@@ -469,7 +472,7 @@ impl RigidBody {
     /// Applies a force at the center-of-mass of this rigid-body.
     pub fn apply_force(&mut self, force: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.linacc += force * self.mass_properties.inv_mass;
+            self.linacc += force * self.effective_inv_mass;
 
             if wake_up {
                 self.wake_up(true);
@@ -480,7 +483,7 @@ impl RigidBody {
     /// Applies an impulse at the center-of-mass of this rigid-body.
     pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.linvel += impulse * self.mass_properties.inv_mass;
+            self.linvel += impulse * self.effective_inv_mass;
 
             if wake_up {
                 self.wake_up(true);
@@ -492,7 +495,8 @@ impl RigidBody {
     #[cfg(feature = "dim2")]
     pub fn apply_torque(&mut self, torque: Real, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
+            self.angacc += self.effective_world_inv_inertia_sqrt
+                * (self.effective_world_inv_inertia_sqrt * torque);
 
             if wake_up {
                 self.wake_up(true);
@@ -504,7 +508,8 @@ impl RigidBody {
     #[cfg(feature = "dim3")]
     pub fn apply_torque(&mut self, torque: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
+            self.angacc += self.effective_world_inv_inertia_sqrt
+                * (self.effective_world_inv_inertia_sqrt * torque);
 
             if wake_up {
                 self.wake_up(true);
@@ -516,8 +521,8 @@ impl RigidBody {
     #[cfg(feature = "dim2")]
     pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angvel +=
-                self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
+            self.angvel += self.effective_world_inv_inertia_sqrt
+                * (self.effective_world_inv_inertia_sqrt * torque_impulse);
 
             if wake_up {
                 self.wake_up(true);
@@ -529,8 +534,8 @@ impl RigidBody {
     #[cfg(feature = "dim3")]
     pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angvel +=
-                self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
+            self.angvel += self.effective_world_inv_inertia_sqrt
+                * (self.effective_world_inv_inertia_sqrt * torque_impulse);
 
             if wake_up {
                 self.wake_up(true);
@@ -679,16 +684,28 @@ impl RigidBodyBuilder {
     }
 
     /// Prevents this rigid-body from rotating because of forces.
-    ///
-    /// This is equivalent to `self.principal_inertia(0.0, false)` (in 2D) or
-    /// `self.principal_inertia(Vector3::zeros(), Vector3::repeat(false))` (in 3D).
-    ///
-    /// See the documentation of [`RigidBodyBuilder::principal_inertia`] for more details.
-    pub fn lock_rotations(self) -> Self {
-        #[cfg(feature = "dim2")]
-        return self.principal_angular_inertia(0.0, false);
-        #[cfg(feature = "dim3")]
-        return self.principal_angular_inertia(Vector::zeros(), Vector::repeat(false));
+    pub fn lock_rotations(mut self) -> Self {
+        self.flags.set(RigidBodyFlags::ROTATION_LOCKED_X, true);
+        self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Y, true);
+        self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Z, true);
+        self
+    }
+
+    /// Only allow rotations of this rigid-body around specific coordinate axes.
+    #[cfg(feature = "dim3")]
+    pub fn restrict_rotations(
+        mut self,
+        allow_rotations_x: bool,
+        allow_rotations_y: bool,
+        allow_rotations_z: bool,
+    ) -> Self {
+        self.flags
+            .set(RigidBodyFlags::ROTATION_LOCKED_X, !allow_rotations_x);
+        self.flags
+            .set(RigidBodyFlags::ROTATION_LOCKED_Y, !allow_rotations_y);
+        self.flags
+            .set(RigidBodyFlags::ROTATION_LOCKED_Z, !allow_rotations_z);
+        self
     }
 
     /// Sets the mass of the rigid-body being built.
@@ -705,42 +722,23 @@ impl RigidBodyBuilder {
     pub fn mass(mut self, mass: Real, colliders_contribution_enabled: bool) -> Self {
         self.mass_properties.inv_mass = utils::inv(mass);
         self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_MASS,
+            RigidBodyFlags::TRANSLATION_LOCKED,
             !colliders_contribution_enabled,
         );
         self
     }
     /// Sets the angular inertia of this rigid-body.
-    ///
-    /// In order to lock the rotations of this rigid-body (by
-    /// making them kinematic), call `.principal_inertia(0.0, false)`.
-    ///
-    /// If `colliders_contribution_enabled` is `false`, then the principal inertia specified here
-    /// will be the final principal inertia of the rigid-body created by this builder.
-    /// If `colliders_contribution_enabled` is `true`, then the final principal of the rigid-body
-    /// will depend on the initial principal inertia set by this method to which is added
-    /// the contributions of all the colliders with non-zero density attached to this rigid-body.
     #[cfg(feature = "dim2")]
-    pub fn principal_angular_inertia(
-        mut self,
-        inertia: Real,
-        colliders_contribution_enabled: bool,
-    ) -> Self {
+    pub fn principal_angular_inertia(mut self, inertia: Real) -> Self {
         self.mass_properties.inv_principal_inertia_sqrt = utils::inv(inertia);
-        self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X
-                | RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y
-                | RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z,
-            !colliders_contribution_enabled,
-        );
         self
     }
 
     /// Use `self.principal_angular_inertia` instead.
     #[cfg(feature = "dim2")]
     #[deprecated(note = "renamed to `principal_angular_inertia`.")]
-    pub fn principal_inertia(self, inertia: Real, colliders_contribution_enabled: bool) -> Self {
-        self.principal_angular_inertia(inertia, colliders_contribution_enabled)
+    pub fn principal_inertia(self, inertia: Real) -> Self {
+        self.principal_angular_inertia(inertia)
     }
 
     /// Sets the principal angular inertia of this rigid-body.
@@ -756,36 +754,16 @@ impl RigidBodyBuilder {
     /// to which is added the contributions of all the colliders with non-zero density
     /// attached to this rigid-body.
     #[cfg(feature = "dim3")]
-    pub fn principal_angular_inertia(
-        mut self,
-        inertia: AngVector<Real>,
-        colliders_contribution_enabled: AngVector<bool>,
-    ) -> Self {
+    pub fn principal_angular_inertia(mut self, inertia: AngVector<Real>) -> Self {
         self.mass_properties.inv_principal_inertia_sqrt = inertia.map(utils::inv);
-        self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X,
-            !colliders_contribution_enabled.x,
-        );
-        self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y,
-            !colliders_contribution_enabled.y,
-        );
-        self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z,
-            !colliders_contribution_enabled.z,
-        );
         self
     }
 
     /// Use `self.principal_angular_inertia` instead.
     #[cfg(feature = "dim3")]
     #[deprecated(note = "renamed to `principal_angular_inertia`.")]
-    pub fn principal_inertia(
-        self,
-        inertia: AngVector<Real>,
-        colliders_contribution_enabled: AngVector<bool>,
-    ) -> Self {
-        self.principal_angular_inertia(inertia, colliders_contribution_enabled)
+    pub fn principal_inertia(self, inertia: AngVector<Real>) -> Self {
+        self.principal_angular_inertia(inertia)
     }
 
     /// Sets the damping factor for the linear part of the rigid-body motion.
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
index 8bc9072..744c00d 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
@@ -27,10 +27,10 @@ impl BallPositionConstraint {
         Self {
             local_com1: rb1.mass_properties.local_com,
             local_com2: rb2.mass_properties.local_com,
-            im1: rb1.mass_properties.inv_mass,
-            im2: rb2.mass_properties.inv_mass,
-            ii1: rb1.world_inv_inertia_sqrt.squared(),
-            ii2: rb2.world_inv_inertia_sqrt.squared(),
+            im1: rb1.effective_inv_mass,
+            im2: rb2.effective_inv_mass,
+            ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
+            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
             position1: rb1.active_set_offset,
@@ -115,8 +115,8 @@ impl BallPositionGroundConstraint {
             // already been flipped by the caller.
             Self {
                 anchor1: rb1.predicted_position * cparams.local_anchor2,
-                im2: rb2.mass_properties.inv_mass,
-                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                im2: rb2.effective_inv_mass,
+                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor1,
                 position2: rb2.active_set_offset,
                 local_com2: rb2.mass_properties.local_com,
@@ -124,8 +124,8 @@ impl BallPositionGroundConstraint {
         } else {
             Self {
                 anchor1: rb1.predicted_position * cparams.local_anchor1,
-                im2: rb2.mass_properties.inv_mass,
-                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                im2: rb2.effective_inv_mass,
+                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor2,
                 position2: rb2.active_set_offset,
                 local_com2: rb2.mass_properties.local_com,
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
index 2f2ffc3..043eea7 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
@@ -31,14 +31,14 @@ impl WBallPositionConstraint {
     ) -> Self {
         let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
         let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
-        let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
         let ii1 = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         )
         .squared();
         let ii2 = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         )
         .squared();
         let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
@@ -141,9 +141,9 @@ impl WBallPositionGroundConstraint {
             } else {
                 cparams[ii].local_anchor1
             }; SIMD_WIDTH]);
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
         let ii2 = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         )
         .squared();
         let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
index 75e19d3..e75f978 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
@@ -40,8 +40,8 @@ impl BallVelocityConstraint {
 
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
 
         let rhs = -(vel1 - vel2);
         let lhs;
@@ -52,12 +52,12 @@ impl BallVelocityConstraint {
         #[cfg(feature = "dim3")]
         {
             lhs = rb2
-                .world_inv_inertia_sqrt
+                .effective_world_inv_inertia_sqrt
                 .squared()
                 .quadform(&cmat2)
                 .add_diagonal(im2)
                 + rb1
-                    .world_inv_inertia_sqrt
+                    .effective_world_inv_inertia_sqrt
                     .squared()
                     .quadform(&cmat1)
                     .add_diagonal(im1);
@@ -67,8 +67,8 @@ impl BallVelocityConstraint {
         // it's just easier that way.
         #[cfg(feature = "dim2")]
         {
-            let ii1 = rb1.world_inv_inertia_sqrt.squared();
-            let ii2 = rb2.world_inv_inertia_sqrt.squared();
+            let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+            let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
             let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
             let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
             let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
@@ -88,8 +88,8 @@ impl BallVelocityConstraint {
             r2: anchor2,
             rhs,
             inv_lhs,
-            ii1_sqrt: rb1.world_inv_inertia_sqrt,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
         }
     }
 
@@ -170,7 +170,7 @@ impl BallVelocityGroundConstraint {
             )
         };
 
-        let im2 = rb2.mass_properties.inv_mass;
+        let im2 = rb2.effective_inv_mass;
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
         let rhs = vel2 - vel1;
@@ -182,7 +182,7 @@ impl BallVelocityGroundConstraint {
         #[cfg(feature = "dim3")]
         {
             lhs = rb2
-                .world_inv_inertia_sqrt
+                .effective_world_inv_inertia_sqrt
                 .squared()
                 .quadform(&cmat2)
                 .add_diagonal(im2);
@@ -190,7 +190,7 @@ impl BallVelocityGroundConstraint {
 
         #[cfg(feature = "dim2")]
         {
-            let ii2 = rb2.world_inv_inertia_sqrt.squared();
+            let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
             let m11 = im2 + cmat2.x * cmat2.x * ii2;
             let m12 = cmat2.x * cmat2.y * ii2;
             let m22 = im2 + cmat2.y * cmat2.y * ii2;
@@ -207,7 +207,7 @@ impl BallVelocityGroundConstraint {
             r2: anchor2,
             rhs,
             inv_lhs,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
         }
     }
 
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
index 697221b..95b0bb5 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
@@ -42,9 +42,9 @@ impl WBallVelocityConstraint {
         let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
         let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
         let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
-        let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
         let ii1_sqrt = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
 
@@ -52,9 +52,9 @@ impl WBallVelocityConstraint {
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
         let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
         let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
@@ -228,9 +228,9 @@ impl WBallVelocityGroundConstraint {
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
         let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
         let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
-        let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
index cf9dcb7..7e8fc97 100644
--- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
@@ -21,10 +21,10 @@ pub(crate) struct FixedPositionConstraint {
 
 impl FixedPositionConstraint {
     pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
-        let ii1 = rb1.world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
         let lin_inv_lhs = 1.0 / (im1 + im2);
         let ang_inv_lhs = (ii1 + ii2).inverse();
 
@@ -111,8 +111,8 @@ impl FixedPositionGroundConstraint {
             anchor1,
             local_anchor2,
             position2: rb2.active_set_offset,
-            im2: rb2.mass_properties.inv_mass,
-            ii2: rb2.world_inv_inertia_sqrt.squared(),
+            im2: rb2.effective_inv_mass,
+            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
             local_com2: rb2.mass_properties.local_com,
             impulse: 0.0,
         }
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
index 715b7dd..2868d4b 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
@@ -51,10 +51,10 @@ impl FixedVelocityConstraint {
     ) -> Self {
         let anchor1 = rb1.position * cparams.local_anchor1;
         let anchor2 = rb2.position * cparams.local_anchor2;
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
-        let ii1 = rb1.world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1.translation.vector - rb1.world_com.coords;
         let r2 = anchor2.translation.vector - rb2.world_com.coords;
         let rmat1 = r1.gcross_matrix();
@@ -118,8 +118,8 @@ impl FixedVelocityConstraint {
             im2,
             ii1,
             ii2,
-            ii1_sqrt: rb1.world_inv_inertia_sqrt,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
             inv_lhs,
             r1,
@@ -248,8 +248,8 @@ impl FixedVelocityGroundConstraint {
 
         let r1 = anchor1.translation.vector - rb1.world_com.coords;
 
-        let im2 = rb2.mass_properties.inv_mass;
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im2 = rb2.effective_inv_mass;
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r2 = anchor2.translation.vector - rb2.world_com.coords;
         let rmat2 = r2.gcross_matrix();
 
@@ -304,7 +304,7 @@ impl FixedVelocityGroundConstraint {
             mj_lambda2: rb2.active_set_offset,
             im2,
             ii2,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
             inv_lhs,
             r2,
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
index 48a6cba..a8d7e91 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
@@ -61,9 +61,9 @@ impl WFixedVelocityConstraint {
         let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
         let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
         let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
-        let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
         let ii1_sqrt = AngularInertia::<SimdReal>::from(
-            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
 
@@ -71,9 +71,9 @@ impl WFixedVelocityConstraint {
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
         let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);