Optional violation correction in velocity solver

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()
         }
     }
 
@@ -173,6 +181,12 @@ impl IntegrationParameters {
             self.dt = 1.0 / inv_dt
         }
     }
+
+    /// Convenience: `velocity_based_erp / dt`
+    #[inline]
+    pub(crate) fn velocity_based_erp_inv_dt(&self) -> Real {
+        self.velocity_based_erp * self.inv_dt()
+    }
 }
 
 impl Default for IntegrationParameters {
@@ -183,6 +197,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,

src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs

@@ -40,17 +40,20 @@ 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 rhs = (vel2 - vel1) * params.velocity_solve_fraction
+            + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
 
+        let lhs;
         let cmat1 = anchor1.gcross_matrix();
         let cmat2 = anchor2.gcross_matrix();
 
@@ -271,22 +274,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 rhs = (vel2 - vel1) * params.velocity_solve_fraction
+            + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
 
         let cmat2 = anchor2.gcross_matrix();
 

src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs

@@ -62,12 +62,15 @@ impl WBallVelocityConstraint {
         let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
         let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
 
-        let anchor1 = position1 * local_anchor1 - world_com1;
-        let anchor2 = position2 * local_anchor2 - world_com2;
+        let anchor_world1 = position1 * local_anchor1;
+        let anchor_world2 = position2 * local_anchor2;
+        let anchor1 = anchor_world1 - world_com1;
+        let anchor2 = anchor_world2 - world_com2;
 
         let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
         let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
-        let rhs = -(vel1 - vel2);
+        let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction)
+            + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt());
         let lhs;
 
         let cmat1 = anchor1.gcross_matrix();
@@ -239,12 +242,15 @@ impl WBallVelocityGroundConstraint {
         );
         let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
 
-        let anchor1 = position1 * local_anchor1 - world_com1;
-        let anchor2 = position2 * local_anchor2 - world_com2;
+        let anchor_world1 = position1 * local_anchor1;
+        let anchor_world2 = position2 * local_anchor2;
+        let anchor1 = anchor_world1 - world_com1;
+        let anchor2 = anchor_world2 - world_com2;
 
         let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
         let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
-        let rhs = vel2 - vel1;
+        let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction)
+            + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt());
         let lhs;
 
         let cmat2 = anchor2.gcross_matrix();

src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs

@@ -103,12 +103,33 @@ 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 =
+            Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction;
 
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let mut rhs = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        ) * params.velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            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 += 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 += 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;
+            }
+        }
 
         FixedVelocityConstraint {
             joint_id,
@@ -293,11 +314,32 @@ 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 =
+            Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction;
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let mut rhs = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        ) * params.velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            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 += 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 += 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;
+            }
+        }
 
         FixedVelocityGroundConstraint {
             joint_id,

src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs

@@ -10,7 +10,7 @@ use crate::math::{
 };
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 #[cfg(feature = "dim3")]
-use na::{Cholesky, Matrix6, Vector6, U3};
+use na::{Cholesky, Matrix6, Vector3, Vector6, U3};
 #[cfg(feature = "dim2")]
 use {
     na::{Matrix3, Vector3},
@@ -132,13 +132,38 @@ impl WFixedVelocityConstraint {
         let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2);
         let ang_dvel = -angvel1 + angvel2;
 
+        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
+
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction;
 
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let mut rhs = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        ) * velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
+
+            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 += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
+            }
+
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err =
+                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                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;
+            }
+        }
 
         WFixedVelocityConstraint {
             joint_id,
@@ -373,12 +398,38 @@ impl WFixedVelocityGroundConstraint {
         let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
         let ang_dvel = angvel2 - angvel1;
 
+        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
+
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction;
+
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let mut rhs = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        ) * velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
+
+            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 += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
+            }
+
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err =
+                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                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;
+            }
+        }
 
         WFixedVelocityGroundConstraint {
             joint_id,