2 files changed, 17 insertions, 27 deletions
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
index 33dc5ba..508ac65 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
@@ -50,9 +50,8 @@ impl BallVelocityConstraint {
         let im1 = rb1.effective_inv_mass;
         let im2 = rb2.effective_inv_mass;
 
-        let mut rhs = params.velocity_solve_fraction * (vel2 - vel1);
-
-        rhs += params.velocity_based_erp * params.inv_dt() * (anchor_world2 - anchor_world1);
+        let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+            + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
 
         let lhs;
         let cmat1 = anchor1.gcross_matrix();
@@ -293,9 +292,9 @@ impl BallVelocityGroundConstraint {
         let im2 = rb2.effective_inv_mass;
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
-        let mut rhs = params.velocity_solve_fraction * (vel2 - vel1);
 
-        rhs += params.velocity_based_erp * params.inv_dt() * (anchor_world2 - anchor_world1);
+        let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+            + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
 
         let cmat2 = anchor2.gcross_matrix();
 
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
index cf710b0..db33fde 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
@@ -112,26 +112,23 @@ impl FixedVelocityConstraint {
                 lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
             );
 
-        if params.velocity_based_erp != 0.0 {
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
             let error = anchor2 * anchor1.inverse();
             let lin_err = error.translation.vector;
 
             #[cfg(feature = "dim2")]
             {
                 let ang_err = error.rotation.angle();
-                rhs += params.velocity_based_erp
-                    * params.inv_dt()
-                    * Vector3::new(lin_err.x, lin_err.y, ang_err);
+                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
             }
 
             #[cfg(feature = "dim3")]
             {
                 let ang_err = error.rotation.scaled_axis();
-                rhs += params.velocity_based_erp
-                    * params.inv_dt()
-                    * Vector6::new(
-                        lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
-                    );
+                rhs += Vector6::new(
+                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+                ) * velocity_based_erp_inv_dt;
             }
         }
 
@@ -326,16 +323,14 @@ impl FixedVelocityGroundConstraint {
                 lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
             );
 
-        if params.velocity_based_erp != 0.0 {
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
             // let error = anchor2 * anchor1.inverse();
             // let lin_err = error.translation.vector;
             // let ang_err = error.rotation;
 
             // Doesn't quite do what it should
-            // let target_pos = anchor1.lerp_slerp(
-            //     &anchor2,
-            //     params.velocity_based_erp * params.inv_dt(),
-            // );
+            // let target_pos = anchor1.lerp_slerp(&anchor2, velocity_based_erp_inv_dt);
             // let error = target_pos * anchor1.inverse();
             // let lin_err = error.translation.vector;
 
@@ -345,19 +340,15 @@ impl FixedVelocityGroundConstraint {
             #[cfg(feature = "dim2")]
             {
                 let ang_err = ang_err.angle();
-                rhs += params.velocity_based_erp
-                    * params.inv_dt()
-                    * Vector3::new(lin_err.x, lin_err.y, ang_err);
+                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
             }
 
             #[cfg(feature = "dim3")]
             {
                 let ang_err = ang_err.scaled_axis();
-                rhs += params.velocity_based_erp
-                    * params.inv_dt()
-                    * Vector6::new(
-                        lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
-                    );
+                rhs += Vector6::new(
+                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+                ) * velocity_based_erp_inv_dt;
             }
         }