Add restorative impulse in velocity solver

2 files changed, 89 insertions, 18 deletions

diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
index a110fbb..33dc5ba 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
@@ -40,17 +40,21 @@ impl BallVelocityConstraint {
         rb2: &RigidBody,
         joint: &BallJoint,
     ) -> Self {
-        let anchor1 = rb1.position * joint.local_anchor1 - rb1.world_com;
-        let anchor2 = rb2.position * joint.local_anchor2 - rb2.world_com;
+        let anchor_world1 = rb1.position * joint.local_anchor1;
+        let anchor_world2 = rb2.position * joint.local_anchor2;
+        let anchor1 = anchor_world1 - rb1.world_com;
+        let anchor2 = anchor_world2 - rb2.world_com;
 
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
         let im1 = rb1.effective_inv_mass;
         let im2 = rb2.effective_inv_mass;
 
-        let rhs = -(vel1 - vel2);
-        let lhs;
+        let mut rhs = params.velocity_solve_fraction * (vel2 - vel1);
+
+        rhs += params.velocity_based_erp * params.inv_dt() * (anchor_world2 - anchor_world1);
 
+        let lhs;
         let cmat1 = anchor1.gcross_matrix();
         let cmat2 = anchor2.gcross_matrix();
 
@@ -271,22 +275,27 @@ impl BallVelocityGroundConstraint {
         joint: &BallJoint,
         flipped: bool,
     ) -> Self {
-        let (anchor1, anchor2) = if flipped {
+        let (anchor_world1, anchor_world2) = if flipped {
             (
-                rb1.position * joint.local_anchor2 - rb1.world_com,
-                rb2.position * joint.local_anchor1 - rb2.world_com,
+                rb1.position * joint.local_anchor2,
+                rb2.position * joint.local_anchor1,
             )
         } else {
             (
-                rb1.position * joint.local_anchor1 - rb1.world_com,
-                rb2.position * joint.local_anchor2 - rb2.world_com,
+                rb1.position * joint.local_anchor1,
+                rb2.position * joint.local_anchor2,
             )
         };
 
+        let anchor1 = anchor_world1 - rb1.world_com;
+        let anchor2 = anchor_world2 - rb2.world_com;
+
         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;
+        let mut rhs = params.velocity_solve_fraction * (vel2 - vel1);
+
+        rhs += params.velocity_based_erp * params.inv_dt() * (anchor_world2 - anchor_world1);
 
         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 2868d4b..cf710b0 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
@@ -103,12 +103,37 @@ impl FixedVelocityConstraint {
         let ang_dvel = -rb1.angvel + rb2.angvel;
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let mut rhs =
+            params.velocity_solve_fraction * Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
 
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        let mut rhs = params.velocity_solve_fraction
+            * Vector6::new(
+                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 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);
+            }
+
+            #[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,
+                    );
+            }
+        }
 
         FixedVelocityConstraint {
             joint_id,
@@ -293,11 +318,48 @@ impl FixedVelocityGroundConstraint {
         let ang_dvel = rb2.angvel - rb1.angvel;
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let mut rhs =
+            params.velocity_solve_fraction * Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        let mut rhs = params.velocity_solve_fraction
+            * Vector6::new(
+                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 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 error = target_pos * anchor1.inverse();
+            // let lin_err = error.translation.vector;
+
+            let lin_err = anchor2.translation.vector - anchor1.translation.vector;
+            let ang_err = anchor2.rotation * anchor1.rotation.inverse();
+
+            #[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);
+            }
+
+            #[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,
+                    );
+            }
+        }
 
         FixedVelocityGroundConstraint {
             joint_id,