Add a twist friction model and velocity-based error correction.

16 files changed, 1555 insertions, 58 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 0d4d3b6..4840ec0 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -19,8 +19,11 @@ pub struct IntegrationParameters {
     /// This allows the user to take action during a timestep, in-between two CCD events.
     pub return_after_ccd_substep: bool,
     /// The Error Reduction Parameter in `[0, 1]` is the proportion of
-    /// the positional error to be corrected at each time step (default: `0.2`).
-    pub erp: Real,
+    /// the positional error to be corrected at the position level each time step (default: `0.0`).
+    pub positionErp: Real,
+    /// The Error Reduction Parameter in `[0, 1]` is the proportion of
+    /// the positional error to be corrected at the velocity level at each time step (default: `0.005`).
+    pub velocityErp: Real,
     /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of
     /// the positional error to be corrected at each time step (default: `0.2`).
     pub joint_erp: Real,
@@ -113,7 +116,7 @@ impl IntegrationParameters {
         IntegrationParameters {
             dt,
             //            multithreading_enabled,
-            erp,
+            positionErp: erp,
             joint_erp,
             warmstart_coeff,
             restitution_velocity_threshold,
@@ -136,6 +139,7 @@ impl IntegrationParameters {
             return_after_ccd_substep,
             multiple_ccd_substep_sensor_events_enabled,
             ccd_on_penetration_enabled,
+            velocityErp: 0.005,
         }
     }
 
@@ -185,7 +189,8 @@ impl Default for IntegrationParameters {
             dt: 1.0 / 60.0,
             //        multithreading_enabled:             true,
             return_after_ccd_substep: false,
-            erp: 0.2,
+            positionErp: 0.0,
+            velocityErp: 0.005,
             joint_erp: 0.2,
             warmstart_coeff: 1.0,
             restitution_velocity_threshold: 1.0,
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index deed8c2..68074e1 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -60,16 +60,18 @@ impl IslandSolver {
         bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt));
         counters.solver.velocity_update_time.pause();
 
-        if manifold_indices.len() != 0 || joint_indices.len() != 0 {
-            counters.solver.position_resolution_time.resume();
-            self.position_solver.solve(
-                island_id,
-                params,
-                bodies,
-                &self.contact_constraints.position_constraints,
-                &self.joint_constraints.position_constraints,
-            );
-            counters.solver.position_resolution_time.pause();
+        if params.positionErp != 0.0 {
+            if manifold_indices.len() != 0 || joint_indices.len() != 0 {
+                counters.solver.position_resolution_time.resume();
+                self.position_solver.solve(
+                    island_id,
+                    params,
+                    bodies,
+                    &self.contact_constraints.position_constraints,
+                    &self.joint_constraints.position_constraints,
+                );
+                counters.solver.position_resolution_time.pause();
+            }
         }
     }
 }
diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs
index 090d0f3..2291615 100644
--- a/src/dynamics/solver/mod.rs
+++ b/src/dynamics/solver/mod.rs
@@ -26,9 +26,15 @@ pub(self) use position_ground_constraint_wide::*;
 pub(self) use velocity_constraint::*;
 #[cfg(feature = "simd-is-enabled")]
 pub(self) use velocity_constraint_wide::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use velocity_constraint_wide_with_manifold_friction::*;
+pub(self) use velocity_constraint_with_manifold_friction::*;
 pub(self) use velocity_ground_constraint::*;
 #[cfg(feature = "simd-is-enabled")]
 pub(self) use velocity_ground_constraint_wide::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use velocity_ground_constraint_wide_with_manifold_friction::*;
+pub(self) use velocity_ground_constraint_with_manifold_friction::*;
 
 mod categorization;
 mod delta_vel;
@@ -57,8 +63,14 @@ mod solver_constraints;
 mod velocity_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod velocity_constraint_wide;
+#[cfg(feature = "simd-is-enabled")]
+mod velocity_constraint_wide_with_manifold_friction;
+mod velocity_constraint_with_manifold_friction;
 mod velocity_ground_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod velocity_ground_constraint_wide;
+#[cfg(feature = "simd-is-enabled")]
+mod velocity_ground_constraint_wide_with_manifold_friction;
+mod velocity_ground_constraint_with_manifold_friction;
 #[cfg(not(feature = "parallel"))]
 mod velocity_solver;
diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs
index 844b1cd..dc5b218 100644
--- a/src/dynamics/solver/position_constraint.rs
+++ b/src/dynamics/solver/position_constraint.rs
@@ -93,7 +93,7 @@ impl PositionConstraint {
                 ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 num_contacts: manifold_points.len() as u8,
-                erp: params.erp,
+                erp: params.positionErp,
                 max_linear_correction: params.max_linear_correction,
             };
 
diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs
index 67b5cdb..8701bfc 100644
--- a/src/dynamics/solver/position_constraint_wide.rs
+++ b/src/dynamics/solver/position_constraint_wide.rs
@@ -74,7 +74,7 @@ impl WPositionConstraint {
                 im2,
                 ii1: sqrt_ii1.squared(),
                 ii2: sqrt_ii2.squared(),
-                erp: SimdReal::splat(params.erp),
+                erp: SimdReal::splat(params.positionErp),
                 max_linear_correction: SimdReal::splat(params.max_linear_correction),
                 num_contacts: num_points as u8,
             };
diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
index 4ab07eb..e69d8ea 100644
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ b/src/dynamics/solver/position_ground_constraint.rs
@@ -66,7 +66,7 @@ impl PositionGroundConstraint {
                 im2: rb2.effective_inv_mass,
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 num_contacts: manifold_contacts.len() as u8,
-                erp: params.erp,
+                erp: params.positionErp,
                 max_linear_correction: params.max_linear_correction,
             };
 
diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs
index f52b3f4..99a469f 100644
--- a/src/dynamics/solver/position_ground_constraint_wide.rs
+++ b/src/dynamics/solver/position_ground_constraint_wide.rs
@@ -71,7 +71,7 @@ impl WPositionGroundConstraint {
                 dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
                 im2,
                 ii2: sqrt_ii2.squared(),
-                erp: SimdReal::splat(params.erp),
+                erp: SimdReal::splat(params.positionErp),
                 max_linear_correction: SimdReal::splat(params.max_linear_correction),
                 num_contacts: num_points as u8,
             };
diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs
index b9dd497..ab8c1e6 100644
--- a/src/dynamics/solver/solver_constraints.rs
+++ b/src/dynamics/solver/solver_constraints.rs
@@ -3,11 +3,15 @@ use super::{
 };
 #[cfg(feature = "simd-is-enabled")]
 use super::{
-    WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint,
+    WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint,
+    WVelocityConstraintWithManifoldFriction, WVelocityGroundConstraint,
+    WVelocityGroundConstraintWithManifoldFriction,
 };
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
 use crate::dynamics::solver::{
-    AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint,
+    AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint,
+    PositionGroundConstraint, VelocityConstraintWithManifoldFriction,
+    VelocityGroundConstraintWithManifoldFriction,
 };
 use crate::dynamics::{
     solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
@@ -122,7 +126,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         {
             let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
             let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
-            WVelocityConstraint::generate(
+            WVelocityConstraintWithManifoldFriction::generate(
                 params,
                 manifold_id,
                 manifolds,
@@ -130,13 +134,16 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 &mut self.velocity_constraints,
                 true,
             );
-            WPositionConstraint::generate(
-                params,
-                manifolds,
-                bodies,
-                &mut self.position_constraints,
-                true,
-            );
+
+            if params.positionErp != 0.0 {
+                WPositionConstraint::generate(
+                    params,
+                    manifolds,
+                    bodies,
+                    &mut self.position_constraints,
+                    true,
+                );
+            }
         }
     }
 
@@ -148,7 +155,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
     ) {
         for manifold_i in &self.interaction_groups.nongrouped_interactions {
             let manifold = &manifolds_all[*manifold_i];
-            VelocityConstraint::generate(
+            VelocityConstraintWithManifoldFriction::generate(
                 params,
                 *manifold_i,
                 manifold,
@@ -156,13 +163,16 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 &mut self.velocity_constraints,
                 true,
             );
-            PositionConstraint::generate(
-                params,
-                manifold,
-                bodies,
-                &mut self.position_constraints,
-                true,
-            );
+
+            if params.positionErp != 0.0 {
+                PositionConstraint::generate(
+                    params,
+                    manifold,
+                    bodies,
+                    &mut self.position_constraints,
+                    true,
+                );
+            }
         }
     }
 
@@ -180,7 +190,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         {
             let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
             let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
-            WVelocityGroundConstraint::generate(
+            WVelocityGroundConstraintWithManifoldFriction::generate(
                 params,
                 manifold_id,
                 manifolds,
@@ -188,13 +198,16 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 &mut self.velocity_constraints,
                 true,
             );
-            WPositionGroundConstraint::generate(
-                params,
-                manifolds,
-                bodies,
-                &mut self.position_constraints,
-                true,
-            );
+
+            if params.positionErp != 0.0 {
+                WPositionGroundConstraint::generate(
+                    params,
+                    manifolds,
+                    bodies,
+                    &mut self.position_constraints,
+                    true,
+                );
+            }
         }
     }
 
@@ -206,7 +219,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
     ) {
         for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
             let manifold = &manifolds_all[*manifold_i];
-            VelocityGroundConstraint::generate(
+            VelocityGroundConstraintWithManifoldFriction::generate(
                 params,
                 *manifold_i,
                 manifold,
@@ -214,13 +227,16 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 &mut self.velocity_constraints,
                 true,
             );
-            PositionGroundConstraint::generate(
-                params,
-                manifold,
-                bodies,
-                &mut self.position_constraints,
-                true,
-            )
+
+            if params.positionErp != 0.0 {
+                PositionGroundConstraint::generate(
+                    params,
+                    manifold,
+                    bodies,
+                    &mut self.position_constraints,
+                    true,
+                )
+            }
         }
     }
 }
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 514434b..aa3ea62 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -1,7 +1,13 @@
 use super::DeltaVel;
-use crate::dynamics::solver::VelocityGroundConstraint;
+use crate::dynamics::solver::{
+    VelocityConstraintWithManifoldFriction, VelocityGroundConstraint,
+    VelocityGroundConstraintWithManifoldFriction,
+};
 #[cfg(feature = "simd-is-enabled")]
-use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
+use crate::dynamics::solver::{
+    WVelocityConstraint, WVelocityConstraintWithManifoldFriction, WVelocityGroundConstraint,
+    WVelocityGroundConstraintWithManifoldFriction,
+};
 use crate::dynamics::{IntegrationParameters, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
@@ -13,10 +19,16 @@ use simba::simd::SimdPartialOrd;
 pub(crate) enum AnyVelocityConstraint {
     NongroupedGround(VelocityGroundConstraint),
     Nongrouped(VelocityConstraint),
+    Nongrouped2(VelocityConstraintWithManifoldFriction),
+    NongroupedGround2(VelocityGroundConstraintWithManifoldFriction),
     #[cfg(feature = "simd-is-enabled")]
     GroupedGround(WVelocityGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
     Grouped(WVelocityConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    Grouped2(WVelocityConstraintWithManifoldFriction),
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedGround2(WVelocityGroundConstraintWithManifoldFriction),
     #[allow(dead_code)] // The Empty variant is only used with parallel code.
     Empty,
 }
@@ -44,10 +56,16 @@ impl AnyVelocityConstraint {
         match self {
             AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas),
             AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas),
+            AnyVelocityConstraint::Nongrouped2(c) => c.warmstart(mj_lambdas),
+            AnyVelocityConstraint::NongroupedGround2(c) => c.warmstart(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::GroupedGround(c) => c.warmstart(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::GroupedGround2(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::Grouped(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::Grouped2(c) => c.warmstart(mj_lambdas),
             AnyVelocityConstraint::Empty => unreachable!(),
         }
     }
@@ -56,10 +74,16 @@ impl AnyVelocityConstraint {
         match self {
             AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas),
             AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas),
+            AnyVelocityConstraint::Nongrouped2(c) => c.solve(mj_lambdas),
+            AnyVelocityConstraint::NongroupedGround2(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::GroupedGround(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::GroupedGround2(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::Grouped2(c) => c.solve(mj_lambdas),
             AnyVelocityConstraint::Empty => unreachable!(),
         }
     }
@@ -68,10 +92,16 @@ impl AnyVelocityConstraint {
         match self {
             AnyVelocityConstraint::NongroupedGround(c) => c.writeback_impulses(manifold_all),
             AnyVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifold_all),
+            AnyVelocityConstraint::Nongrouped2(c) => c.writeback_impulses(manifold_all),
+            AnyVelocityConstraint::NongroupedGround2(c) => c.writeback_impulses(manifold_all),
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all),
             #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::GroupedGround2(c) => c.writeback_impulses(manifold_all),
+            #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::Grouped2(c) => c.writeback_impulses(manifold_all),
             AnyVelocityConstraint::Empty => unreachable!(),
         }
     }
diff --git a/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs b/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs
new file mode 100644
index 0000000..d9d3e53
--- /dev/null
+++ b/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs
@@ -0,0 +1,416 @@
+use super::{AnyVelocityConstraint, DeltaVel};
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{
+    AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
+use num::Zero;
+use simba::simd::{SimdPartialOrd, SimdValue};
+
+#[derive(Copy, Clone, Debug)]
+struct WVelocityConstraintPart {
+    gcross1: AngVector<SimdReal>,
+    gcross2: AngVector<SimdReal>,
+    rhs: SimdReal,
+    impulse: SimdReal,
+    r: SimdReal,
+}
+
+impl WVelocityConstraintPart {
+    pub fn zero() -> Self {
+        Self {
+            gcross1: AngVector::zero(),
+            gcross2: AngVector::zero(),
+            rhs: SimdReal::zero(),
+            impulse: SimdReal::zero(),
+            r: SimdReal::zero(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityConstraintWithManifoldFriction {
+    dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
+    normal_parts: [WVelocityConstraintPart; MAX_MANIFOLD_POINTS],
+    tangent_parts: [WVelocityConstraintPart; DIM - 1],
+    twist_part: WVelocityConstraintPart,
+    twist_weights: [SimdReal; MAX_MANIFOLD_POINTS],
+    num_contacts: u8,
+    im1: SimdReal,
+    im2: SimdReal,
+    limit: SimdReal,
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WIDTH],
+    manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+    manifold_contact_id: usize,
+}
+
+impl WVelocityConstraintWithManifoldFriction {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+        manifolds: [&ContactManifold; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
+        push: bool,
+    ) {
+        let inv_dt = SimdReal::splat(params.inv_dt());
+        let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
+        let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
+
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii1: AngularInertia<SimdReal> = AngularInertia::from(
+            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2: AngularInertia<SimdReal> = AngularInertia::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+
+        let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
+
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
+
+        let warmstart_multiplier =
+            SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
+        let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
+        let num_active_contacts = manifolds[0].data.num_active_contacts();
+
+        for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
+            let manifold_points = array![|ii|
+                &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH
+            ];
+            let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
+
+            let mut constraint = WVelocityConstraintWithManifoldFriction {
+                dir1: force_dir1,
+                normal_parts: [WVelocityConstraintPart::zero(); MAX_MANIFOLD_POINTS],
+                tangent_parts: [WVelocityConstraintPart::zero(); DIM - 1],
+                twist_part: WVelocityConstraintPart::zero(),
+                twist_weights: [SimdReal::splat(0.0); MAX_MANIFOLD_POINTS],
+                im1,
+                im2,
+                limit: SimdReal::splat(0.0),
+                mj_lambda1,
+                mj_lambda2,
+                manifold_id,
+                manifold_contact_id: l,
+                num_contacts: num_points as u8,
+            };
+
+            let mut manifold_center = Point::origin();
+            let mut tangent_impulses = [SimdReal::splat(0.0), SimdReal::splat(0.0)];
+
+            for k in 0..num_points {
+                let friction =
+                    SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
+                let restitution =
+                    SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
+                let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
+
+                manifold_center += point.coords / SimdReal::splat(num_points as Real);
+
+                let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
+
+                let impulse =
+                    SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
+                tangent_impulses[0] += SimdReal::from(
+                    array![|ii| manifold_points[ii][k].data.tangent_impulse[0]; SIMD_WIDTH],
+                );
+                tangent_impulses[1] += SimdReal::from(
+                    array![|ii| manifold_points[ii][k].data.tangent_impulse[1]; SIMD_WIDTH],
+                );
+
+                let dp1 = point - world_com1;
+                let dp2 = point - world_com2;
+
+                let vel1 = linvel1 + angvel1.gcross(dp1);
+                let vel2 = linvel2 + angvel2.gcross(dp2);
+
+                constraint.limit = friction;
+
+                // Normal part.
+                {
+                    let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
+                    let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
+
+                    let r = SimdReal::splat(1.0)
+                        / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
+                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
+                    let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
+                    let rhs_with_restitution = rhs + rhs * restitution;
+                    rhs = rhs_with_restitution.select(use_restitution, rhs);
+
+                    rhs += ((dist.simd_min(SimdReal::splat(0.0))
+                        * SimdReal::splat(params.velocityErp))
+                        + dist.simd_max(SimdReal::splat(0.0)))
+                        * inv_dt;
+                    // rhs += dist.simd_max(SimdReal::splat(-0.01)) * inv_dt;
+
+                    constraint.normal_parts[k] = WVelocityConstraintPart {
+                        gcross1,
+                        gcross2,
+                        rhs,
+                        impulse: impulse * warmstart_coeff,
+                        r,
+                    };
+                }
+            }
+
+            // tangent parts.
+            let tangents1 = force_dir1.orthonormal_basis();
+
+            for j in 0..DIM - 1 {
+                let dp1 = manifold_center - world_com1;
+                let dp2 = manifold_center - world_com2;
+
+                let vel1 = linvel1 + angvel1.gcross(dp1);
+                let vel2 = linvel2 + angvel2.gcross(dp2);
+
+                let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
+                let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
+                let r = SimdReal::splat(1.0)
+                    / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
+                let rhs = (vel1 - vel2).dot(&tangents1[j]);
+
+                constraint.tangent_parts[j] = WVelocityConstraintPart {
+                    gcross1,
+                    gcross2,
+                    rhs,
+                    impulse: tangent_impulses[j] * warmstart_coeff,
+                    r,
+                };
+            }
+
+            // Twist part.
+            {
+                let twist_impulse =
+                    SimdReal::from(array![|ii| manifolds[ii].data.twist_impulse; SIMD_WIDTH]);
+
+                for k in 0..num_points {
+                    let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
+                    constraint.twist_weights[k] = na::distance(&point, &manifold_center);
+                }
+
+                let gcross1 = ii1.transform_vector(force_dir1);
+                let gcross2 = ii2.transform_vector(-force_dir1);
+                let inv_r = gcross1.norm_squared() + gcross2.norm_squared();
+                let rhs = (angvel1 - angvel2).gdot(force_dir1);
+                constraint.twist_part = WVelocityConstraintPart {
+                    gcross1,
+                    gcross2,
+                    rhs,
+                    impulse: twist_impulse * warmstart_coeff,
+                    r: crate::utils::simd_inv(inv_r),
+                }
+            }
+
+            if push {
+                out_constraints.push(AnyVelocityConstraint::Grouped2(constraint));
+            } else {
+                out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    AnyVelocityConstraint::Grouped2(constraint);
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        // Normal part.
+        for i in 0..self.num_contacts as usize {
+            let elt = &self.normal_parts[i];
+            mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse);
+            mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
+            mj_lambda2.angular += elt.gcross2 * elt.impulse;
+        }
+
+        // Tangent part.
+        {
+            let tangents1 = self.dir1.orthonormal_basis();
+
+            for j in 0..DIM - 1 {
+                let elt = &self.tangent_parts[j];
+                mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse);
+                mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
+                mj_lambda2.angular += elt.gcross2 * elt.impulse;
+            }
+        }
+
+        // Twist part.
+        {
+            mj_lambda1.angular += self.twist_part.gcross1 * self.twist_part.impulse;
+            mj_lambda2.angular += self.twist_part.gcross2 * self.twist_part.impulse;
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        // Solve friction first.
+        let tangents1 = self.dir1.orthonormal_basis();
+        let friction_limit = self.limit
+            * (self.normal_parts[0].impulse
+                + self.normal_parts[1].impulse
+                + self.normal_parts[2].impulse
+                + self.normal_parts[3].impulse);
+
+        for j in 0..DIM - 1 {
+            let elt = &mut self.tangent_parts[j];
+            let dimpulse = tangents1[j].dot(&mj_lambda1.linear)
+                + elt.gcross1.gdot(mj_lambda1.angular)