Merge pull request #276 from dimforge/lock-translation-axis

Allow locking individual translational axes

13 files changed, 101 insertions, 80 deletions

diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs
index fb06335..8c93511 100644
--- a/src/dynamics/solver/generic_velocity_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_constraint.rs
@@ -116,12 +116,12 @@ impl GenericVelocityConstraint {
                 im1: if rb_type1.is_dynamic() {
                     rb_mprops1.effective_inv_mass
                 } else {
-                    0.0
+                    na::zero()
                 },
                 im2: if rb_type2.is_dynamic() {
                     rb_mprops2.effective_inv_mass
                 } else {
-                    0.0
+                    na::zero()
                 },
                 limit: 0.0,
                 mj_lambda1,
@@ -175,7 +175,8 @@ impl GenericVelocityConstraint {
                         )
                         .0
                     } else if rb_type1.is_dynamic() {
-                        rb_mprops1.effective_inv_mass + gcross1.gdot(gcross1)
+                        force_dir1.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1))
+                            + gcross1.gdot(gcross1)
                     } else {
                         0.0
                     };
@@ -193,7 +194,8 @@ impl GenericVelocityConstraint {
                         )
                         .0
                     } else if rb_type2.is_dynamic() {
-                        rb_mprops2.effective_inv_mass + gcross2.gdot(gcross2)
+                        force_dir1.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1))
+                            + gcross2.gdot(gcross2)
                     } else {
                         0.0
                     };
@@ -258,7 +260,9 @@ impl GenericVelocityConstraint {
                             )
                             .0
                         } else if rb_type1.is_dynamic() {
-                            rb_mprops1.effective_inv_mass + gcross1.gdot(gcross1)
+                            force_dir1
+                                .dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1))
+                                + gcross1.gdot(gcross1)
                         } else {
                             0.0
                         };
@@ -276,7 +280,9 @@ impl GenericVelocityConstraint {
                             )
                             .0
                         } else if rb_type2.is_dynamic() {
-                            rb_mprops2.effective_inv_mass + gcross2.gdot(gcross2)
+                            force_dir1
+                                .dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1))
+                                + gcross2.gdot(gcross2)
                         } else {
                             0.0
                         };
@@ -345,8 +351,8 @@ impl GenericVelocityConstraint {
             &self.velocity_constraint.dir1,
             #[cfg(feature = "dim3")]
             &self.velocity_constraint.tangent1,
-            self.velocity_constraint.im1,
-            self.velocity_constraint.im2,
+            &self.velocity_constraint.im1,
+            &self.velocity_constraint.im2,
             self.velocity_constraint.limit,
             self.ndofs1,
             self.ndofs2,
diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs
index 150be8b..c31bbfb 100644
--- a/src/dynamics/solver/generic_velocity_constraint_element.rs
+++ b/src/dynamics/solver/generic_velocity_constraint_element.rs
@@ -70,11 +70,11 @@ impl GenericRhs {
         dir: &Vector<Real>,
         gcross: &AngVector<Real>,
         mj_lambdas: &mut DVector<Real>,
-        inv_mass: Real,
+        inv_mass: &Vector<Real>,
     ) {
         match self {
             GenericRhs::DeltaVel(rhs) => {
-                rhs.linear += dir * (inv_mass * impulse);
+                rhs.linear += dir.component_mul(inv_mass) * impulse;
                 rhs.angular += gcross * impulse;
             }
             GenericRhs::GenericId(mj_lambda) => {
@@ -94,8 +94,8 @@ impl VelocityConstraintTangentPart<Real> {
         j_id: usize,
         jacobians: &DVector<Real>,
         tangents1: [&Vector<Real>; DIM - 1],
-        im1: Real,
-        im2: Real,
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
         ndofs1: usize,
         ndofs2: usize,
         limit: Real,
@@ -246,8 +246,8 @@ impl VelocityConstraintNormalPart<Real> {
         j_id: usize,
         jacobians: &DVector<Real>,
         dir1: &Vector<Real>,
-        im1: Real,
-        im2: Real,
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
         ndofs1: usize,
         ndofs2: usize,
         mj_lambda1: &mut GenericRhs,
@@ -296,8 +296,8 @@ impl VelocityConstraintElement<Real> {
         jacobians: &DVector<Real>,
         dir1: &Vector<Real>,
         #[cfg(feature = "dim3")] tangent1: &Vector<Real>,
-        im1: Real,
-        im2: Real,
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
         limit: Real,
         // ndofs is 0 for a non-multibody body, or a multibody with zero
         // degrees of freedom.
diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
index 92bcc9f..5edc815 100644
--- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
@@ -33,7 +33,7 @@ impl SolverBody<Real, 1> {
             let mut out_invm_j = jacobians.fixed_rows_mut::<SPATIAL_DIM>(wj_id);
             out_invm_j
                 .fixed_rows_mut::<DIM>(0)
-                .axpy(self.im, &unit_force, 0.0);
+                .copy_from(&self.im.component_mul(&unit_force));
 
             #[cfg(feature = "dim2")]
             {
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
index 3e31256..a4ec313 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
@@ -51,7 +51,7 @@ pub enum WritebackId {
 pub struct SolverBody<N: SimdRealField, const LANES: usize> {
     pub linvel: Vector<N>,
     pub angvel: AngVector<N>,
-    pub im: N,
+    pub im: Vector<N>,
     pub sqrt_ii: AngularInertia<N>,
     pub world_com: Point<N>,
     pub mj_lambda: [usize; LANES],
@@ -74,8 +74,8 @@ pub struct JointVelocityConstraint<N: SimdRealField, const LANES: usize> {
     pub rhs: N,
     pub rhs_wo_bias: N,
 
-    pub im1: N,
-    pub im2: N,
+    pub im1: Vector<N>,
+    pub im2: Vector<N>,
 
     pub writeback_id: WritebackId,
 }
@@ -94,8 +94,8 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
             inv_lhs: N::zero(),
             rhs: N::zero(),
             rhs_wo_bias: N::zero(),
-            im1: N::zero(),
-            im2: N::zero(),
+            im1: na::zero(),
+            im2: na::zero(),
             writeback_id: WritebackId::Dof(0),
         }
     }
@@ -115,9 +115,9 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
         let ang_impulse1 = self.ang_jac1 * delta_impulse;
         let ang_impulse2 = self.ang_jac2 * delta_impulse;
 
-        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.linear += lin_impulse.component_mul(&self.im1);
         mj_lambda1.angular += ang_impulse1;
-        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.linear -= lin_impulse.component_mul(&self.im2);
         mj_lambda2.angular -= ang_impulse2;
     }
 
@@ -353,7 +353,7 @@ pub struct JointVelocityGroundConstraint<N: SimdRealField, const LANES: usize> {
     pub rhs: N,
     pub rhs_wo_bias: N,
 
-    pub im2: N,
+    pub im2: Vector<N>,
 
     pub writeback_id: WritebackId,
 }
@@ -370,7 +370,7 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
             inv_lhs: N::zero(),
             rhs: N::zero(),
             rhs_wo_bias: N::zero(),
-            im2: N::zero(),
+            im2: na::zero(),
             writeback_id: WritebackId::Dof(0),
         }
     }
@@ -388,7 +388,7 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
         let lin_impulse = self.lin_jac * delta_impulse;
         let ang_impulse = self.ang_jac2 * delta_impulse;
 
-        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.linear -= lin_impulse.component_mul(&self.im2);
         mj_lambda2.angular -= ang_impulse;
     }
 
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
index dd17ecf..55a112a 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
@@ -355,7 +355,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         // Use the modified Gram-Schmidt orthogonalization.
         for j in 0..len {
             let c_j = &mut constraints[j];
-            let dot_jj = c_j.lin_jac.norm_squared() * imsum
+            let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                 + c_j.ang_jac1.gdot(c_j.ang_jac1)
                 + c_j.ang_jac2.gdot(c_j.ang_jac2);
             let inv_dot_jj = crate::utils::simd_inv(dot_jj);
@@ -370,7 +370,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
             for i in (j + 1)..len {
                 let (c_i, c_j) = constraints.index_mut_const(i, j);
-                let dot_ij = c_i.lin_jac.dot(&c_j.lin_jac) * imsum
+                let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                     + c_i.ang_jac1.gdot(c_j.ang_jac1)
                     + c_i.ang_jac2.gdot(c_j.ang_jac2);
                 let coeff = dot_ij * inv_dot_jj;
@@ -672,7 +672,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         // Use the modified Gram-Schmidt orthogonalization.
         for j in 0..len {
             let c_j = &mut constraints[j];
-            let dot_jj = c_j.lin_jac.norm_squared() * imsum + c_j.ang_jac2.gdot(c_j.ang_jac2);
+            let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+                + c_j.ang_jac2.gdot(c_j.ang_jac2);
             let inv_dot_jj = crate::utils::simd_inv(dot_jj);
             c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
 
@@ -685,8 +686,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
             for i in (j + 1)..len {
                 let (c_i, c_j) = constraints.index_mut_const(i, j);
-                let dot_ij =
-                    c_i.lin_jac.dot(&c_j.lin_jac) * imsum + c_i.ang_jac2.gdot(c_j.ang_jac2);
+                let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+                    + c_i.ang_jac2.gdot(c_j.ang_jac2);
                 let coeff = dot_ij * inv_dot_jj;
 
                 c_i.lin_jac -= c_j.lin_jac * coeff;
diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs
index e695b3d..6d5debe 100644
--- a/src/dynamics/solver/parallel_island_solver.rs
+++ b/src/dynamics/solver/parallel_island_solver.rs
@@ -247,7 +247,7 @@ impl ParallelIslandSolver {
                             // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
                             //       by the square root of the inertia tensor:
                             dvel.angular += rb_mass_props.effective_world_inv_inertia_sqrt * rb_forces.torque * params.dt;
-                            dvel.linear += rb_forces.force * (rb_mass_props.effective_inv_mass * params.dt);
+                            dvel.linear += rb_forces.force.component_mul(&rb_mass_props.effective_inv_mass) * params.dt;
                         }
                     }
 
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 719468d..0a841b8 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -94,8 +94,8 @@ pub(crate) struct VelocityConstraint {
     pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
     #[cfg(feature = "dim3")]
     pub tangent1: Vector<Real>, // One of the friction force directions.
-    pub im1: Real,
-    pub im2: Real,
+    pub im1: Vector<Real>,
+    pub im2: Vector<Real>,
     pub limit: Real,
     pub mj_lambda1: usize,
     pub mj_lambda2: usize,
@@ -235,9 +235,9 @@ impl VelocityConstraint {
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
+                    let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
                     let r = 1.0
-                        / (mprops1.effective_inv_mass
-                            + mprops2.effective_inv_mass
+                        / (force_dir1.dot(&imsum.component_mul(&force_dir1))
                             + gcross1.gdot(gcross1)
                             + gcross2.gdot(gcross2));
 
@@ -274,9 +274,9 @@ impl VelocityConstraint {
                         let gcross2 = mprops2
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dp2.gcross(-tangents1[j]));
+                        let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
                         let r = 1.0
-                            / (mprops1.effective_inv_mass
-                                + mprops2.effective_inv_mass
+                            / (tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
                                 + gcross1.gdot(gcross1)
                                 + gcross2.gdot(gcross2));
                         let rhs =
@@ -314,8 +314,8 @@ impl VelocityConstraint {
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
-            self.im1,
-            self.im2,
+            &self.im1,
+            &self.im2,
             self.limit,
             &mut mj_lambda1,
             &mut mj_lambda2,
diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs
index 406f68e..e153626 100644
--- a/src/dynamics/solver/velocity_constraint_element.rs
+++ b/src/dynamics/solver/velocity_constraint_element.rs
@@ -30,8 +30,8 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
     pub fn solve(
         &mut self,
         tangents1: [&Vector<N>; DIM - 1],
-        im1: N,
-        im2: N,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
         limit: N,
         mj_lambda1: &mut DeltaVel<N>,
         mj_lambda2: &mut DeltaVel<N>,
@@ -50,10 +50,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
             let dlambda = new_impulse - self.impulse[0];
             self.impulse[0] = new_impulse;
 
-            mj_lambda1.linear += tangents1[0] * (im1 * dlambda);
+            mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda;
             mj_lambda1.angular += self.gcross1[0] * dlambda;
 
-            mj_lambda2.linear += tangents1[0] * (-im2 * dlambda);
+            mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda;
             mj_lambda2.angular += self.gcross2[0] * dlambda;
         }
 
@@ -84,12 +84,12 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
             let dlambda = new_impulse - self.impulse;
             self.impulse = new_impulse;
 
-            mj_lambda1.linear +=
-                tangents1[0] * (im1 * dlambda[0]) + tangents1[1] * (im1 * dlambda[1]);
+            mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+                + tangents1[1].component_mul(im1) * dlambda[1];
             mj_lambda1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
 
-            mj_lambda2.linear +=
-                tangents1[0] * (-im2 * dlambda[0]) + tangents1[1] * (-im2 * dlambda[1]);
+            mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+                + tangents1[1].component_mul(im2) * -dlambda[1];
             mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
         }
     }
@@ -121,8 +121,8 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
     pub fn solve(
         &mut self,
         dir1: &Vector<N>,
-        im1: N,
-        im2: N,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
         mj_lambda1: &mut DeltaVel<N>,
         mj_lambda2: &mut DeltaVel<N>,
     ) where
@@ -136,10 +136,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
-        mj_lambda1.linear += dir1 * (im1 * dlambda);
+        mj_lambda1.linear += dir1.component_mul(im1) * dlambda;
         mj_lambda1.angular += self.gcross1 * dlambda;
 
-        mj_lambda2.linear += dir1 * (-im2 * dlambda);
+        mj_lambda2.linear += dir1.component_mul(im2) * -dlambda;
         mj_lambda2.angular += self.gcross2 * dlambda;
     }
 }
@@ -163,8 +163,8 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
         elements: &mut [Self],
         dir1: &Vector<N>,
         #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im1: N,
-        im2: N,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
         limit: N,
         mj_lambda1: &mut DeltaVel<N>,
         mj_lambda2: &mut DeltaVel<N>,
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 0e2e36a..f072ad8 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -20,8 +20,8 @@ pub(crate) struct WVelocityConstraint {
     pub tangent1: Vector<SimdReal>, // One of the friction force directions.
     pub elements: [VelocityConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
-    pub im1: SimdReal,
-    pub im2: SimdReal,
+    pub im1: Vector<SimdReal>,
+    pub im2: Vector<SimdReal>,
     pub limit: SimdReal,
     pub mj_lambda1: [usize; SIMD_WIDTH],
     pub mj_lambda2: [usize; SIMD_WIDTH],
@@ -62,7 +62,7 @@ impl WVelocityConstraint {
         let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
 
         let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
-        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
+        let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);
         let ii1: AngularInertia<SimdReal> =
             AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
 
@@ -70,7 +70,7 @@ impl WVelocityConstraint {
         let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
 
         let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
+        let im2 = Vector::from(gather![|ii| mprops2[ii].effective_inv_mass]);
         let ii2: AngularInertia<SimdReal> =
             AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
 
@@ -135,8 +135,11 @@ impl WVelocityConstraint {
                     let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
+                    let imsum = im1 + im2;
                     let r = SimdReal::splat(1.0)
-                        / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
+                        / (force_dir1.dot(&imsum.component_mul(&force_dir1))
+                            + gcross1.gdot(gcross1)
+                            + gcross2.gdot(gcross2));
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
                     let mut rhs_wo_bias =
                         (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
@@ -161,8 +164,11 @@ impl WVelocityConstraint {
                 for j in 0..DIM - 1 {
                     let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
                     let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
+                    let imsum = im1 + im2;
                     let r = SimdReal::splat(1.0)
-                        / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
+                        / (tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+                            + gcross1.gdot(gcross1)
+                            + gcross2.gdot(gcross2));
                     let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]);
 
                     constraint.elements[k].tangent_part.gcross1[j] = gcross1;
@@ -206,8 +212,8 @@ impl WVelocityConstraint {
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
-            self.im1,
-            self.im2,
+            &self.im1,
+            &self.im2,
             self.limit,
             &mut mj_lambda1,
             &mut mj_lambda2,
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 87865b3..bd60633 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -17,7 +17,7 @@ pub(crate) struct VelocityGroundConstraint {
     pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
     #[cfg(feature = "dim3")]
     pub tangent1: Vector<Real>, // One of the friction force directions.
-    pub im2: Real,
+    pub im2: Vector<Real>,
     pub limit: Real,
     pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS],
 
@@ -153,7 +153,9 @@ impl VelocityGroundConstraint {
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
-                    let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2));
+                    let r = 1.0
+                        / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+                            + gcross2.gdot(gcross2));
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
                     let is_resting = 1.0 - is_bouncy;
@@ -184,7 +186,10 @@ impl VelocityGroundConstraint {
                         let gcross2 = mprops2
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dp2.gcross(-tangents1[j]));
-                        let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2));
+                        let r = 1.0
+                            / (tangents1[j]
+                                .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+                                + gcross2.gdot(gcross2));
                         let rhs = (vel1 - vel2
                             + flipped_multiplier * manifold_point.tangent_velocity)
                             .dot(&tangents1[j]);
@@ -219,7 +224,7 @@ impl VelocityGroundConstraint {
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
-            self.im2,
+            &self.im2,
             self.limit,
             &mut mj_lambda2,
             solve_normal,
diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index adfcfda..4d4d3c3 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_element.rs
@@ -29,7 +29,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
     pub fn solve(
         &mut self,
         tangents1: [&Vector<N>; DIM - 1],
-        im2: N,
+        im2: &Vector<N>,
         limit: N,
         mj_lambda2: &mut DeltaVel<N>,
     ) where
@@ -45,7 +45,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
             let dlambda = new_impulse - self.impulse[0];
             self.impulse[0] = new_impulse;
 
-            mj_lambda2.linear += tangents1[0] * (-im2 * dlambda);
+            mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda;
             mj_lambda2.angular += self.gcross2[0] * dlambda;
         }
 
@@ -72,8 +72,8 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
 
             self.impulse = new_impulse;
 
-            mj_lambda2.linear +=
-                tangents1[0] * (-im2 * dlambda[0]) + tangents1[1] * (-im2 * dlambda[1]);
+            mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+                + tangents1[1].component_mul(im2) * -dlambda[1];
             mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
         }
     }
@@ -101,7 +101,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
     }
 
     #[inline]
-    pub fn solve(&mut self, dir1: &Vector<N>, im2: N, mj_lambda2: &mut DeltaVel<N>)
+    pub fn solve(&mut self, dir1: &Vector<N>, im2: &Vector<N>, mj_lambda2: &mut DeltaVel<N>)
     where
         AngVector<N>: WDot<AngVector<N>, Result = N>,
     {
@@ -111,7 +111,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
-        mj_lambda2.linear += dir1 * (-im2 * dlambda);
+        mj_lambda2.linear += dir1.component_mul(im2) * -dlambda;
         mj_lambda2.angular += self.gcross2 * dlambda;
     }
 }
@@ -136,7 +136,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
         elements: &mut [Self],
         dir1: &Vector<N>,
         #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im2: N,
+        im2: &Vector<N>,
         limit: N,
         mj_lambda2: &mut DeltaVel<N>,
         solve_normal: bool,
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index e1ea8f6..2e4812b 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -21,7 +21,7 @@ pub(crate) struct WVelocityGroundConstraint {
     pub tangent1: Vector<SimdReal>, // One of the friction force directions.
     pub elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
-    pub im2: SimdReal,
+    pub im2: Vector<SimdReal>,
     pub limit: SimdReal,
     pub mj_lambda2: [usize; SIMD_WIDTH],
     pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
@@ -76,7 +76,7 @@ impl WVelocityGroundConstraint {
 
         let flipped_sign = SimdReal::from(flipped);
 
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
+        let im2 = Vector::from(gather![|ii| mprops2[ii].effective_inv_mass]);
         let ii2: AngularInertia<SimdReal> =
             AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
 
@@ -142,7 +142,8 @@ impl WVelocityGroundConstraint {
                 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
-                    let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
+                    let r = SimdReal::splat(1.0)
+                        / (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2));
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
                     let mut rhs_wo_bias =
                         (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
@@ -165,7 +166,9 @@ impl WVelocityGroundConstraint {
 
                 for j in 0..DIM - 1 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
-                    let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
+                    let r = SimdReal::splat(1.0)
+                        / (tangents1[j].dot(&im2.component_mul(&tangents1[j]))
+                            + gcross2.gdot(gcross2));
                     let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
 
                     constraint.elements[k].tangent_part.gcross2[j] = gcross2;
@@ -201,7 +204,7 @@ impl WVelocityGroundConstraint {
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
-            self.im2,
+            &self.im2,
             self.limit,
             &mut mj_lambda2,
             solve_normal,
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index ed1fe3a..91b6be9 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -67,7 +67,7 @@ impl VelocitySolver {
             // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
             //       by the square root of the inertia tensor:
             dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
-            dvel.linear += forces.force * (mprops.effective_inv_mass * params.dt);
+            dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
         }
 
         for (_, multibody) in multibodies.multibodies.iter_mut() {