Add restorative impulse in velocity solver

5 files changed, 125 insertions, 34 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index caad9b5..96d9eaf 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -27,6 +27,19 @@ pub struct IntegrationParameters {
     /// Each cached impulse are multiplied by this coefficient in `[0, 1]`
     /// when they are re-used to initialize the solver (default `1.0`).
     pub warmstart_coeff: Real,
+
+    /// 0-1: how much of the velocity to dampen out in the constraint solver?
+    /// (default `1.0`).
+    pub velocity_solve_fraction: Real,
+
+    /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
+    /// will be compensated for during the velocity solve.
+    /// If zero, you need to enable the positional solver.
+    /// If non-zero, you do not need the positional solver.
+    /// A good non-zero value is around `0.2`.
+    /// (default `0.0`).
+    pub velocity_based_erp: Real,
+
     /// Amount of penetration the engine wont attempt to correct (default: `0.005m`).
     pub allowed_linear_error: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
@@ -121,17 +134,12 @@ impl IntegrationParameters {
             max_stabilization_multiplier,
             max_velocity_iterations,
             max_position_iterations,
-            // FIXME: what is the optimal value for min_island_size?
-            // It should not be too big so that we don't end up with
-            // huge islands that don't fit in cache.
-            // However we don't want it to be too small and end up with
-            // tons of islands, reducing SIMD parallelism opportunities.
-            min_island_size: 128,
             max_ccd_position_iterations,
             max_ccd_substeps,
             return_after_ccd_substep,
             multiple_ccd_substep_sensor_events_enabled,
             ccd_on_penetration_enabled,
+            ..Default::default()
         }
     }
 
@@ -183,6 +191,8 @@ impl Default for IntegrationParameters {
             return_after_ccd_substep: false,
             erp: 0.2,
             joint_erp: 0.2,
+            velocity_solve_fraction: 1.0,
+            velocity_based_erp: 0.0,
             warmstart_coeff: 1.0,
             allowed_linear_error: 0.005,
             prediction_distance: 0.002,
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,
                     };
                 }