Add restorative impulse in velocity solver

4 files changed, 109 insertions, 28 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,
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index abc46c9..dff8ff0 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -244,17 +244,22 @@ impl VelocityConstraint {
                             + gcross2.gdot(gcross2));
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
-                    let rhs = (1.0 + is_bouncy * manifold_point.restitution)
-                        * (vel1 - vel2).dot(&force_dir1)
-                        + manifold_point.dist.max(0.0) * inv_dt;
+                    let is_resting = 1.0 - is_bouncy;
 
-                    let impulse = manifold_point.data.impulse * warmstart_coeff;
+                    let mut rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                        * (vel1 - vel2).dot(&force_dir1);
+                    rhs += manifold_point.dist.max(0.0) * inv_dt;
+                    rhs *= params.velocity_solve_fraction;
+                    rhs += is_resting
+                        * params.velocity_based_erp
+                        * inv_dt
+                        * manifold_point.dist.min(0.0);
 
                     constraint.elements[k].normal_part = VelocityConstraintElementPart {
                         gcross1,
                         gcross2,
                         rhs,
-                        impulse,
+                        impulse: manifold_point.data.impulse * warmstart_coeff,
                         r,
                     };
                 }
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index a3a5563..b3d4eeb 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -156,16 +156,21 @@ impl VelocityGroundConstraint {
                     let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
-                    let rhs = (1.0 + is_bouncy * manifold_point.restitution)
-                        * (vel1 - vel2).dot(&force_dir1)
-                        + manifold_point.dist.max(0.0) * inv_dt;
+                    let is_resting = 1.0 - is_bouncy;
 
-                    let impulse = manifold_point.data.impulse * warmstart_coeff;
+                    let mut rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                        * (vel1 - vel2).dot(&force_dir1);
+                    rhs += manifold_point.dist.max(0.0) * inv_dt;
+                    rhs *= params.velocity_solve_fraction;
+                    rhs += is_resting
+                        * params.velocity_based_erp
+                        * inv_dt
+                        * manifold_point.dist.min(0.0);
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
                         gcross2,
                         rhs,
-                        impulse,
+                        impulse: manifold_point.data.impulse * warmstart_coeff,
                         r,
                     };
                 }