feat: implement new "small-steps" solver + joint improvements

12 files changed, 3893 insertions, 0 deletions

diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
new file mode 100644
index 0000000..4d02270
--- /dev/null
+++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
@@ -0,0 +1,528 @@
+use crate::dynamics::solver::categorization::categorize_contacts;
+use crate::dynamics::solver::contact_constraint::{
+    GenericOneBodyConstraint, GenericOneBodyConstraintBuilder, GenericTwoBodyConstraint,
+    GenericTwoBodyConstraintBuilder, OneBodyConstraint, OneBodyConstraintBuilder,
+    TwoBodyConstraint, TwoBodyConstraintBuilder,
+};
+use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::solver_vel::SolverVel;
+use crate::dynamics::solver::{reset_buffer, ConstraintTypes, SolverConstraintsSet};
+use crate::dynamics::{
+    ImpulseJoint, IntegrationParameters, IslandManager, JointAxesMask, MultibodyJointSet,
+    RigidBodySet,
+};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{Real, MAX_MANIFOLD_POINTS};
+use na::DVector;
+use parry::math::DIM;
+
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::dynamics::solver::contact_constraint::{
+        OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd,
+        TwoBodyConstraintSimd,
+    },
+    crate::math::SIMD_WIDTH,
+};
+
+pub struct ConstraintsCounts {
+    pub num_constraints: usize,
+    pub num_jacobian_lines: usize,
+}
+
+impl ConstraintsCounts {
+    pub fn from_contacts(manifold: &ContactManifold) -> Self {
+        let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
+        Self {
+            num_constraints: manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS
+                + rest as usize,
+            num_jacobian_lines: manifold.data.solver_contacts.len() * DIM,
+        }
+    }
+
+    pub fn from_joint(joint: &ImpulseJoint) -> Self {
+        let joint = &joint.data;
+        let locked_axes = joint.locked_axes.bits();
+        let motor_axes = joint.motor_axes.bits() & !locked_axes;
+        let limit_axes = joint.limit_axes.bits() & !locked_axes;
+        let coupled_axes = joint.coupled_axes.bits();
+
+        let num_constraints = (motor_axes & !coupled_axes).count_ones() as usize
+            + ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+            + ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize
+            + locked_axes.count_ones() as usize
+            + (limit_axes & !coupled_axes).count_ones() as usize
+            + ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+            + ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize;
+        Self {
+            num_constraints,
+            num_jacobian_lines: num_constraints,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct ContactConstraintTypes;
+
+impl ConstraintTypes for ContactConstraintTypes {
+    type OneBody = OneBodyConstraint;
+    type TwoBodies = TwoBodyConstraint;
+    type GenericOneBody = GenericOneBodyConstraint;
+    type GenericTwoBodies = GenericTwoBodyConstraint;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdOneBody = OneBodyConstraintSimd;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdTwoBodies = TwoBodyConstraintSimd;
+
+    type BuilderOneBody = OneBodyConstraintBuilder;
+    type BuilderTwoBodies = TwoBodyConstraintBuilder;
+    type GenericBuilderOneBody = GenericOneBodyConstraintBuilder;
+    type GenericBuilderTwoBodies = GenericTwoBodyConstraintBuilder;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdBuilderOneBody = SimdOneBodyConstraintBuilder;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdBuilderTwoBodies = TwoBodyConstraintBuilderSimd;
+}
+
+pub type ContactConstraintsSet = SolverConstraintsSet<ContactConstraintTypes>;
+
+impl ContactConstraintsSet {
+    pub fn init_constraint_groups(
+        &mut self,
+        island_id: usize,
+        islands: &IslandManager,
+        bodies: &RigidBodySet,
+        multibody_joints: &MultibodyJointSet,
+        manifolds: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+    ) {
+        self.two_body_interactions.clear();
+        self.one_body_interactions.clear();
+        self.generic_two_body_interactions.clear();
+        self.generic_one_body_interactions.clear();
+
+        categorize_contacts(
+            bodies,
+            multibody_joints,
+            manifolds,
+            manifold_indices,
+            &mut self.one_body_interactions,
+            &mut self.two_body_interactions,
+            &mut self.generic_one_body_interactions,
+            &mut self.generic_two_body_interactions,
+        );
+
+        self.interaction_groups.clear_groups();
+        self.interaction_groups.group_manifolds(
+            island_id,
+            islands,
+            bodies,
+            manifolds,
+            &self.two_body_interactions,
+        );
+
+        self.one_body_interaction_groups.clear_groups();
+        self.one_body_interaction_groups.group_manifolds(
+            island_id,
+            islands,
+            bodies,
+            manifolds,
+            &self.one_body_interactions,
+        );
+
+        // NOTE: uncomment this do disable SIMD contact resolution.
+        //        self.interaction_groups
+        //            .nongrouped_interactions
+        //            .append(&mut self.interaction_groups.simd_interactions);
+        //        self.one_body_interaction_groups
+        //            .nongrouped_interactions
+        //            .append(&mut self.one_body_interaction_groups.simd_interactions);
+    }
+
+    pub fn init(
+        &mut self,
+        island_id: usize,
+        islands: &IslandManager,
+        bodies: &RigidBodySet,
+        multibody_joints: &MultibodyJointSet,
+        manifolds: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+    ) {
+        self.clear_constraints();
+        self.clear_builders();
+
+        self.init_constraint_groups(
+            island_id,
+            islands,
+            bodies,
+            multibody_joints,
+            manifolds,
+            manifold_indices,
+        );
+
+        let mut jacobian_id = 0;
+
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.simd_compute_constraints(bodies, manifolds);
+        }
+        self.compute_constraints(bodies, manifolds);
+        self.compute_generic_constraints(bodies, multibody_joints, manifolds, &mut jacobian_id);
+
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.simd_compute_one_body_constraints(bodies, manifolds);
+        }
+        self.compute_one_body_constraints(bodies, manifolds);
+        self.compute_generic_one_body_constraints(
+            bodies,
+            multibody_joints,
+            manifolds,
+            &mut jacobian_id,
+        );
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn simd_compute_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        let total_num_constraints = self
+            .interaction_groups
+            .simd_interactions
+            .chunks_exact(SIMD_WIDTH)
+            .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints)
+            .sum();
+
+        unsafe {
+            reset_buffer(
+                &mut self.simd_velocity_constraints_builder,
+                total_num_constraints,
+            );
+            reset_buffer(&mut self.simd_velocity_constraints, total_num_constraints);
+        }
+
+        let mut curr_start = 0;
+
+        for manifolds_i in self
+            .interaction_groups
+            .simd_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let num_to_add =
+                ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints;
+            let manifold_id = gather![|ii| manifolds_i[ii]];
+            let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
+
+            TwoBodyConstraintBuilderSimd::generate(
+                manifold_id,
+                manifolds,
+                bodies,
+                &mut self.simd_velocity_constraints_builder[curr_start..],
+                &mut self.simd_velocity_constraints[curr_start..],
+            );
+
+            curr_start += num_to_add;
+        }
+
+        assert_eq!(curr_start, total_num_constraints);
+    }
+
+    fn compute_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        let total_num_constraints = self
+            .interaction_groups
+            .nongrouped_interactions
+            .iter()
+            .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints)
+            .sum();
+
+        unsafe {
+            reset_buffer(
+                &mut self.velocity_constraints_builder,
+                total_num_constraints,
+            );
+            reset_buffer(&mut self.velocity_constraints, total_num_constraints);
+        }
+
+        let mut curr_start = 0;
+
+        for manifold_i in &self.interaction_groups.nongrouped_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
+
+            TwoBodyConstraintBuilder::generate(
+                *manifold_i,
+                manifold,
+                bodies,
+                &mut self.velocity_constraints_builder[curr_start..],
+                &mut self.velocity_constraints[curr_start..],
+            );
+
+            curr_start += num_to_add;
+        }
+        assert_eq!(curr_start, total_num_constraints);
+    }
+
+    fn compute_generic_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        multibody_joints: &MultibodyJointSet,
+        manifolds_all: &[&mut ContactManifold],
+        jacobian_id: &mut usize,
+    ) {
+        let total_num_constraints = self
+            .generic_two_body_interactions
+            .iter()
+            .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints)
+            .sum();
+
+        self.generic_velocity_constraints_builder.resize(
+            total_num_constraints,
+            GenericTwoBodyConstraintBuilder::invalid(),
+        );
+        self.generic_velocity_constraints
+            .resize(total_num_constraints, GenericTwoBodyConstraint::invalid());
+
+        let mut curr_start = 0;
+
+        for manifold_i in &self.generic_two_body_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
+
+            GenericTwoBodyConstraintBuilder::generate(
+                *manifold_i,
+                manifold,
+                bodies,
+                multibody_joints,
+                &mut self.generic_velocity_constraints_builder[curr_start..],
+                &mut self.generic_velocity_constraints[curr_start..],
+                &mut self.generic_jacobians,
+                jacobian_id,
+            );
+
+            curr_start += num_to_add;
+        }
+
+        assert_eq!(curr_start, total_num_constraints);
+    }
+
+    fn compute_generic_one_body_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        multibody_joints: &MultibodyJointSet,
+        manifolds_all: &[&mut ContactManifold],
+        jacobian_id: &mut usize,
+    ) {
+        let total_num_constraints = self
+            .generic_one_body_interactions
+            .iter()
+            .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints)
+            .sum();
+        self.generic_velocity_one_body_constraints_builder.resize(
+            total_num_constraints,
+            GenericOneBodyConstraintBuilder::invalid(),
+        );
+        self.generic_velocity_one_body_constraints
+            .resize(total_num_constraints, GenericOneBodyConstraint::invalid());
+
+        let mut curr_start = 0;
+
+        for manifold_i in &self.generic_one_body_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
+
+            GenericOneBodyConstraintBuilder::generate(
+                *manifold_i,
+                manifold,
+                bodies,
+                multibody_joints,
+                &mut self.generic_velocity_one_body_constraints_builder[curr_start..],
+                &mut self.generic_velocity_one_body_constraints[curr_start..],
+                &mut self.generic_jacobians,
+                jacobian_id,
+            );
+
+            curr_start += num_to_add;
+        }
+        assert_eq!(curr_start, total_num_constraints);
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn simd_compute_one_body_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        let total_num_constraints = self
+            .one_body_interaction_groups
+            .simd_interactions
+            .chunks_exact(SIMD_WIDTH)
+            .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints)
+            .sum();
+
+        unsafe {
+            reset_buffer(
+                &mut self.simd_velocity_one_body_constraints_builder,
+                total_num_constraints,
+            );
+            reset_buffer(
+                &mut self.simd_velocity_one_body_constraints,
+                total_num_constraints,
+            );
+        }
+
+        let mut curr_start = 0;
+
+        for manifolds_i in self
+            .one_body_interaction_groups
+            .simd_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let num_to_add =
+                ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints;
+            let manifold_id = gather![|ii| manifolds_i[ii]];
+            let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
+            SimdOneBodyConstraintBuilder::generate(
+                manifold_id,
+                manifolds,
+                bodies,
+                &mut self.simd_velocity_one_body_constraints_builder[curr_start..],
+                &mut self.simd_velocity_one_body_constraints[curr_start..],
+            );
+            curr_start += num_to_add;
+        }
+        assert_eq!(curr_start, total_num_constraints);
+    }
+
+    fn compute_one_body_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        let total_num_constraints = self
+            .one_body_interaction_groups
+            .nongrouped_interactions
+            .iter()
+            .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints)
+            .sum();
+
+        unsafe {
+            reset_buffer(
+                &mut self.velocity_one_body_constraints_builder,
+                total_num_constraints,
+            );
+            reset_buffer(
+                &mut self.velocity_one_body_constraints,
+                total_num_constraints,
+            );
+        }
+
+        let mut curr_start = 0;
+
+        for manifold_i in &self.one_body_interaction_groups.nongrouped_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
+
+            OneBodyConstraintBuilder::generate(
+                *manifold_i,
+                manifold,
+                bodies,
+                &mut self.velocity_one_body_constraints_builder[curr_start..],
+                &mut self.velocity_one_body_constraints[curr_start..],
+            );
+
+            curr_start += num_to_add;
+        }
+        assert_eq!(curr_start, total_num_constraints);
+    }
+
+    pub fn solve_restitution(
+        &mut self,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let (jac, constraints) = self.iter_constraints_mut();
+        for mut c in constraints {
+            c.solve_restitution(jac, solver_vels, generic_solver_vels);
+        }
+    }
+
+    pub fn solve_restitution_wo_bias(
+        &mut self,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let (jac, constraints) = self.iter_constraints_mut();
+        for mut c in constraints {
+            c.remove_bias();
+            c.solve_restitution(jac, solver_vels, generic_solver_vels);
+        }
+    }
+
+    pub fn solve_friction(
+        &mut self,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let (jac, constraints) = self.iter_constraints_mut();
+        for mut c in constraints {
+            c.solve_friction(jac, solver_vels, generic_solver_vels);
+        }
+    }
+
+    pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) {
+        let (_, constraints) = self.iter_constraints_mut();
+        for mut c in constraints {
+            c.writeback_impulses(manifolds_all);
+        }
+    }
+
+    pub fn update(
+        &mut self,
+        params: &IntegrationParameters,
+        small_step_id: usize,
+        multibodies: &MultibodyJointSet,
+        solver_bodies: &[SolverBody],
+    ) {
+        macro_rules! update_contacts(
+            ($builders: ident, $constraints: ident) => {
+                for (builder, constraint) in self.$builders.iter().zip(self.$constraints.iter_mut()) {
+                    builder.update(
+                        &params,
+                        small_step_id as Real * params.dt,
+                        solver_bodies,
+                        multibodies,
+                        constraint,
+                    );
+                }
+            }
+        );
+
+        update_contacts!(
+            generic_velocity_constraints_builder,
+            generic_velocity_constraints
+        );
+        update_contacts!(velocity_constraints_builder, velocity_constraints);
+        #[cfg(feature = "simd-is-enabled")]
+        update_contacts!(simd_velocity_constraints_builder, simd_velocity_constraints);
+
+        update_contacts!(
+            generic_velocity_one_body_constraints_builder,
+            generic_velocity_one_body_constraints
+        );
+        update_contacts!(
+            velocity_one_body_constraints_builder,
+            velocity_one_body_constraints
+        );
+        #[cfg(feature = "simd-is-enabled")]
+        update_contacts!(
+            simd_velocity_one_body_constraints_builder,
+            simd_velocity_one_body_constraints
+        );
+    }
+}
diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
new file mode 100644
index 0000000..7c39eab
--- /dev/null
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
@@ -0,0 +1,292 @@
+use crate::dynamics::solver::OneBodyConstraint;
+use crate::dynamics::{
+    IntegrationParameters, MultibodyJointSet, MultibodyLinkId, RigidBodySet, RigidBodyVelocity,
+};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
+use crate::utils::SimdCross;
+
+use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
+use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::{ContactPointInfos, OneBodyConstraintBuilder};
+#[cfg(feature = "dim2")]
+use crate::utils::SimdBasis;
+use na::DVector;
+
+#[derive(Copy, Clone)]
+pub(crate) struct GenericOneBodyConstraintBuilder {
+    link2: MultibodyLinkId,
+    ccd_thickness: Real,
+    inner: OneBodyConstraintBuilder,
+}
+
+impl GenericOneBodyConstraintBuilder {
+    pub fn invalid() -> Self {
+        Self {
+            link2: MultibodyLinkId::default(),
+            ccd_thickness: 0.0,
+            inner: OneBodyConstraintBuilder::invalid(),
+        }
+    }
+
+    pub fn generate(
+        manifold_id: ContactManifoldIndex,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        multibodies: &MultibodyJointSet,
+        out_builders: &mut [GenericOneBodyConstraintBuilder],
+        out_constraints: &mut [GenericOneBodyConstraint],
+        jacobians: &mut DVector<Real>,
+        jacobian_id: &mut usize,
+    ) {
+        let mut handle1 = manifold.data.rigid_body1;
+        let mut handle2 = manifold.data.rigid_body2;
+        let flipped = manifold.data.relative_dominance < 0;
+
+        let (force_dir1, flipped_multiplier) = if flipped {
+            std::mem::swap(&mut handle1, &mut handle2);
+            (manifold.data.normal, -1.0)
+        } else {
+            (-manifold.data.normal, 1.0)
+        };
+
+        let (vels1, world_com1) = if let Some(handle1) = handle1 {
+            let rb1 = &bodies[handle1];
+            (rb1.vels, rb1.mprops.world_com)
+        } else {
+            (RigidBodyVelocity::zero(), Point::origin())
+        };
+
+        let rb1 = handle1
+            .map(|h| SolverBody::from(&bodies[h]))
+            .unwrap_or_else(SolverBody::default);
+
+        let rb2 = &bodies[handle2.unwrap()];
+        let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
+
+        let link2 = *multibodies.rigid_body_link(handle2.unwrap()).unwrap();
+        let (mb2, link_id2) = (&multibodies[link2.multibody], link2.id);
+        let solver_vel2 = mb2.solver_id;
+
+        #[cfg(feature = "dim2")]
+        let tangents1 = force_dir1.orthonormal_basis();
+        #[cfg(feature = "dim3")]
+        let tangents1 =
+            super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
+
+        let multibodies_ndof = mb2.ndofs();
+        // For each solver contact we generate DIM constraints, and each constraints appends
+        // the multibodies jacobian and weighted jacobians
+        let required_jacobian_len =
+            *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
+
+        if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
+            jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
+        }
+
+        for (l, manifold_points) in manifold
+            .data
+            .solver_contacts
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            let chunk_j_id = *jacobian_id;
+
+            let builder = &mut out_builders[l];
+            let constraint = &mut out_constraints[l];
+
+            builder.inner.rb1 = rb1;
+            builder.inner.vels1 = vels1;
+
+            constraint.inner.dir1 = force_dir1;
+            constraint.inner.im2 = mprops2.effective_inv_mass;
+            constraint.inner.solver_vel2 = solver_vel2;
+            constraint.inner.manifold_id = manifold_id;
+            constraint.inner.num_contacts = manifold_points.len() as u8;
+            #[cfg(feature = "dim3")]
+            {
+                constraint.inner.tangent1 = tangents1[0];
+            }
+
+            for k in 0..manifold_points.len() {
+                let manifold_point = &manifold_points[k];
+                let point = manifold_point.point;
+                let dp1 = point - world_com1;
+                let dp2 = point - mprops2.world_com;
+
+                let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
+                let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
+
+                constraint.inner.limit = manifold_point.friction;
+                constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
+
+                // Normal part.
+                let normal_rhs_wo_bias;
+                {
+                    let torque_dir2 = dp2.gcross(-force_dir1);
+                    let inv_r2 = mb2
+                        .fill_jacobians(
+                            link_id2,
+                            -force_dir1,
+                            #[cfg(feature = "dim2")]
+                            na::vector!(torque_dir2),
+                            #[cfg(feature = "dim3")]
+                            torque_dir2,
+                            jacobian_id,
+                            jacobians,
+                        )
+                        .0;
+
+                    let r = crate::utils::inv(inv_r2);
+
+                    let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
+
+                    let proj_vel1 = vel1.dot(&force_dir1);
+                    let proj_vel2 = vel2.dot(&force_dir1);
+                    let dvel = proj_vel1 - proj_vel2;
+                    // NOTE: we add proj_vel1 since it’s not accessible through solver_vel.
+                    normal_rhs_wo_bias =
+                        proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel;
+
+                    constraint.inner.elements[k].normal_part = OneBodyConstraintNormalPart {
+                        gcross2: na::zero(), // Unused for generic constraints.
+                        rhs: na::zero(),
+                        rhs_wo_bias: na::zero(),
+                        impulse: na::zero(),
+                        total_impulse: na::zero(),
+                        r,
+                    };
+                }
+
+                // Tangent parts.
+                {
+                    constraint.inner.elements[k].tangent_part.impulse = na::zero();
+
+                    for j in 0..DIM - 1 {
+                        let torque_dir2 = dp2.gcross(-tangents1[j]);
+                        let inv_r2 = mb2
+                            .fill_jacobians(
+                                link_id2,
+                                -tangents1[j],
+                                #[cfg(feature = "dim2")]
+                                na::vector![torque_dir2],
+                                #[cfg(feature = "dim3")]
+                                torque_dir2,
+                                jacobian_id,
+                                jacobians,
+                            )
+                            .0;
+
+                        let r = crate::utils::inv(inv_r2);
+
+                        let rhs_wo_bias = (vel1
+                            + flipped_multiplier * manifold_point.tangent_velocity)
+                            .dot(&tangents1[j]);
+
+                        constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
+                        constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
+
+                        // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
+                        // in lhs. See the corresponding code on the `velocity_constraint.rs`
+                        // file.
+                        constraint.inner.elements[k].tangent_part.r[j] = r;
+                    }
+                }
+
+                // Builder.
+                let infos = ContactPointInfos {
+                    local_p1: rb1.position.inverse_transform_point(&manifold_point.point),
+                    local_p2: rb2
+                        .pos
+                        .position
+                        .inverse_transform_point(&manifold_point.point),
+                    tangent_vel: manifold_point.tangent_velocity,
+                    dist: manifold_point.dist,
+                    normal_rhs_wo_bias,
+                };
+
+                builder.link2 = link2;
+                builder.ccd_thickness = rb2.ccd.ccd_thickness;
+                builder.inner.infos[k] = infos;
+                constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
+            }
+
+            constraint.j_id = chunk_j_id;
+            constraint.ndofs2 = mb2.ndofs();
+        }
+    }
+
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        solved_dt: Real,
+        _solver_bodies: &[SolverBody],
+        multibodies: &MultibodyJointSet,
+        constraint: &mut GenericOneBodyConstraint,
+    ) {
+        // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint.
+        let pos2 = &multibodies[self.link2.multibody]
+            .link(self.link2.id)
+            .unwrap()
+            .local_to_world;
+
+        self.inner.update_with_positions(
+            params,
+            solved_dt,
+            pos2,
+            self.ccd_thickness,
+            &mut constraint.inner,
+        );
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct GenericOneBodyConstraint {
+    // We just build the generic constraint on top of the velocity constraint,
+    // adding some information we can use in the generic case.
+    pub inner: OneBodyConstraint,
+    pub j_id: usize,
+    pub ndofs2: usize,
+}
+
+impl GenericOneBodyConstraint {
+    pub fn invalid() -> Self {
+        Self {
+            inner: OneBodyConstraint::invalid(),
+            j_id: usize::MAX,
+            ndofs2: usize::MAX,
+        }
+    }
+
+    pub fn solve(
+        &mut self,
+        jacobians: &DVector<Real>,
+        generic_solver_vels: &mut DVector<Real>,
+        solve_restitution: bool,
+        solve_friction: bool,
+    ) {
+        let solver_vel2 = self.inner.solver_vel2 as usize;
+
+        let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
+        OneBodyConstraintElement::generic_solve_group(
+            self.inner.cfm_factor,
+            elements,
+            jacobians,
+            self.inner.limit,
+            self.ndofs2,
+            self.j_id,
+            solver_vel2,
+            generic_solver_vels,
+            solve_restitution,
+            solve_friction,
+        );
+    }
+
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        self.inner.writeback_impulses(manifolds_all);
+    }
+
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.inner.remove_cfm_and_bias_from_rhs();
+    }
+}
diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
new file mode 100644
index 0000000..1003a9a
--- /dev/null
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
@@ -0,0 +1,147 @@
+use crate::dynamics::solver::{
+    OneBodyConstraintElement, OneBodyConstraintNormalPart, OneBodyConstraintTangentPart,
+};
+use crate::math::{Real, DIM};
+use na::DVector;
+#[cfg(feature = "dim2")]
+use na::SimdPartialOrd;
+
+impl OneBodyConstraintTangentPart<Real> {
+    #[inline]
+    pub fn generic_solve(
+        &mut self,
+        j_id2: usize,
+        jacobians: &DVector<Real>,
+        ndofs2: usize,
+        limit: Real,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        #[cfg(feature = "dim2")]
+        {
+            let dvel_0 = jacobians
+                .rows(j_id2, ndofs2)
+                .dot(&solver_vels.rows(solver_vel2, ndofs2))
+                + self.rhs[0];
+
+            let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
+            let dlambda = new_impulse - self.impulse[0];
+            self.impulse[0] = new_impulse;
+
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
+                dlambda,
+                &jacobians.rows(j_id2 + ndofs2, ndofs2),