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

src/dynamics/ccd/mod.rs

@@ -1,3 +1,6 @@
+// TODO: not sure why it complains about PredictedImpacts being unused,
+//       making it private or pub(crate) triggers a different error.
+#[allow(unused_imports)]
 pub use self::ccd_solver::{CCDSolver, PredictedImpacts};
 pub use self::toi_entry::TOIEntry;
 

src/dynamics/integration_parameters.rs

@@ -1,4 +1,5 @@
 use crate::math::Real;
+use std::num::NonZeroUsize;
 
 /// Parameters for a time-step of the physics engine.
 #[derive(Copy, Clone, Debug)]
@@ -43,15 +44,10 @@ pub struct IntegrationParameters {
     pub max_penetration_correction: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
     pub prediction_distance: Real,
-    /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`).
-    pub max_velocity_iterations: usize,
-    /// Maximum number of iterations performed to solve friction constraints (default: `8`).
-    pub max_velocity_friction_iterations: usize,
-    /// Maximum number of iterations performed to remove the energy introduced by penetration corrections  (default: `1`).
-    pub max_stabilization_iterations: usize,
-    /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise,
-    /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`).
-    pub interleave_restitution_and_friction_resolution: bool,
+    /// Number of iterations performed to solve friction constraints at solver iteration (default: `2`).
+    pub num_friction_iteration_per_solver_iteration: usize,
+    /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
+    pub num_solver_iterations: NonZeroUsize,
     /// Minimum number of dynamic bodies in each active island (default: `128`).
     pub min_island_size: usize,
     /// Maximum number of substeps performed by the  solver (default: `1`).
@@ -151,17 +147,15 @@ impl Default for IntegrationParameters {
         Self {
             dt: 1.0 / 60.0,
             min_ccd_dt: 1.0 / 60.0 / 100.0,
-            erp: 0.8,
-            damping_ratio: 0.25,
+            erp: 0.6,
+            damping_ratio: 1.0,
             joint_erp: 1.0,
             joint_damping_ratio: 1.0,
-            allowed_linear_error: 0.001, // 0.005
+            allowed_linear_error: 0.001,
             max_penetration_correction: Real::MAX,
             prediction_distance: 0.002,
-            max_velocity_iterations: 4,
-            max_velocity_friction_iterations: 8,
-            max_stabilization_iterations: 1,
-            interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability.
+            num_friction_iteration_per_solver_iteration: 2,
+            num_solver_iterations: NonZeroUsize::new(4).unwrap(),
             // TODO: 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.

src/dynamics/island_manager.rs

@@ -4,7 +4,7 @@ use crate::dynamics::{
 };
 use crate::geometry::{ColliderSet, NarrowPhase};
 use crate::math::Real;
-use crate::utils::WDot;
+use crate::utils::SimdDot;
 
 /// Structure responsible for maintaining the set of active rigid-bodies, and
 /// putting non-moving rigid-bodies to sleep to save computation times.
@@ -14,6 +14,7 @@ pub struct IslandManager {
     pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
     pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
     pub(crate) active_islands: Vec<usize>,
+    pub(crate) active_islands_additional_solver_iterations: Vec<usize>,
     active_set_timestamp: u32,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     can_sleep: Vec<RigidBodyHandle>, // Workspace.
@@ -28,6 +29,7 @@ impl IslandManager {
             active_dynamic_set: vec![],
             active_kinematic_set: vec![],
             active_islands: vec![],
+            active_islands_additional_solver_iterations: vec![],
             active_set_timestamp: 0,
             can_sleep: vec![],
             stack: vec![],
@@ -127,6 +129,10 @@ impl IslandManager {
         &self.active_dynamic_set[island_range]
     }
 
+    pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize {
+        self.active_islands_additional_solver_iterations[island_id]
+    }
+
     #[inline(always)]
     pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a {
         self.active_dynamic_set
@@ -232,12 +238,21 @@ impl IslandManager {
         //        let t = instant::now();
         // Propagation of awake state and awake island computation through the
         // traversal of the interaction graph.
+        self.active_islands_additional_solver_iterations.clear();
         self.active_islands.clear();
         self.active_islands.push(0);
 
         // The max avoid underflow when the stack is empty.
         let mut island_marker = self.stack.len().max(1) - 1;
 
+        // NOTE: islands containing a body with non-standard number of iterations won’t
+        //       be merged with another island, unless another island with standard
+        //       iterations number already started before and got continued due to the
+        //       `min_island_size`. That could be avoided by pushing bodies with non-standard
+        //       iterations on top of the stack (and other bodies on the back). Not sure it’s
+        //       worth it though.
+        let mut additional_solver_iterations = 0;
+
         while let Some(handle) = self.stack.pop() {
             let rb = bodies.index_mut_internal(handle);
 
@@ -248,16 +263,23 @@ impl IslandManager {
             }
 
             if self.stack.len() < island_marker {
-                if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
-                    >= min_island_size
+                if additional_solver_iterations != rb.additional_solver_iterations
+                    || self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
+                        >= min_island_size
                 {
                     // We are starting a new island.
+                    self.active_islands_additional_solver_iterations
+                        .push(additional_solver_iterations);
                     self.active_islands.push(self.active_dynamic_set.len());
+                    additional_solver_iterations = 0;
                 }
 
                 island_marker = self.stack.len();
             }
 
+            additional_solver_iterations =
+                additional_solver_iterations.max(rb.additional_solver_iterations);
+
             // Transmit the active state to all the rigid-bodies with colliders
             // in contact or joined with this collider.
             push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack);
@@ -281,6 +303,8 @@ impl IslandManager {
             self.active_dynamic_set.push(handle);
         }
 
+        self.active_islands_additional_solver_iterations
+            .push(additional_solver_iterations);
         self.active_islands.push(self.active_dynamic_set.len());
         //        println!(
         //            "Extraction: {}, num islands: {}",

src/dynamics/joint/generic_joint.rs

@@ -1,7 +1,7 @@
 use crate::dynamics::solver::MotorParameters;
 use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
 use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM};
-use crate::utils::{WBasis, WReal};
+use crate::utils::{SimdBasis, SimdRealCopy};
 
 #[cfg(feature = "dim3")]
 use crate::dynamics::SphericalJoint;
@@ -121,7 +121,7 @@ pub struct JointLimits<N> {
     pub impulse: N,
 }
 
-impl<N: WReal> Default for JointLimits<N> {
+impl<N: SimdRealCopy> Default for JointLimits<N> {
     fn default() -> Self {
         Self {
             min: -N::splat(Real::MAX),
@@ -131,6 +131,16 @@ impl<N: WReal> Default for JointLimits<N> {
     }
 }
 
+impl<N: SimdRealCopy> From<[N; 2]> for JointLimits<N> {
+    fn from(value: [N; 2]) -> Self {
+        Self {
+            min: value[0],
+            max: value[1],
+            impulse: N::splat(0.0),
+        }
+    }
+}
+
 /// A joint’s motor along one of its degrees of freedom.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
@@ -210,14 +220,23 @@ pub struct GenericJoint {
     /// The degrees-of-freedoms motorised by this joint.
     pub motor_axes: JointAxesMask,
     /// The coupled degrees of freedom of this joint.
+    ///
+    /// Note that coupling degrees of freedoms (DoF) changes the interpretation of the coupled joint’s limits and motors.
+    /// If multiple linear DoF are limited/motorized, only the limits/motor configuration for the first
+    /// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized
+    /// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF.
     pub coupled_axes: JointAxesMask,
     /// The limits, along each degrees of freedoms of this joint.
     ///
     /// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask.
+    /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis`
+    /// bitmask is applied to the coupled linear (resp. angular) axes.
     pub limits: [JointLimits<Real>; SPATIAL_DIM],
     /// The motors, along each degrees of freedoms of this joint.
     ///
-    /// Note that the mostor must also be explicitly enabled by the `motors` bitmask.
+    /// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask.
+    /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes`
+    /// bitmask is applied to the coupled linear (resp. angular) axes.
     pub motors: [JointMotor; SPATIAL_DIM],
     /// Are contacts between the attached rigid-bodies enabled?
     pub contacts_enabled: bool,

src/dynamics/joint/impulse_joint/impulse_joint_set.rs

@@ -214,7 +214,7 @@ impl ImpulseJointSet {
     //     //     .map(|e| &mut e.weight)
     // }
 
-    #[cfg(not(feature = "parallel"))]
+    // #[cfg(not(feature = "parallel"))]
     pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] {
         &mut self.joint_graph.graph.edges[..]
     }

src/dynamics/joint/mod.rs

@@ -6,6 +6,7 @@ pub use self::multibody_joint::*;
 pub use self::prismatic_joint::*;
 pub use self::revolute_joint::*;
 pub use self::rope_joint::*;
+pub use self::spring_joint::*;
 
 #[cfg(feature = "dim3")]
 pub use self::spherical_joint::*;
@@ -21,3 +22,4 @@ mod rope_joint;
 
 #[cfg(feature = "dim3")]
 mod spherical_joint;
+mod spring_joint;

src/dynamics/joint/multibody_joint/mod.rs

@@ -2,7 +2,9 @@
 
 pub use self::multibody::Multibody;
 pub use self::multibody_joint::MultibodyJoint;
-pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet};
+pub use self::multibody_joint_set::{
+    MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId,
+};
 pub use self::multibody_link::MultibodyLink;
 pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint};
 

src/dynamics/joint/multibody_joint/multibody.rs

@@ -1,16 +1,13 @@
 use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
 use super::multibody_workspace::MultibodyWorkspace;
-use crate::dynamics::{
-    solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet,
-    RigidBodyType, RigidBodyVelocity,
-};
+use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity};
 #[cfg(feature = "dim3")]
 use crate::math::Matrix;
 use crate::math::{
     AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
 };
 use crate::prelude::MultibodyJoint;
-use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix};
+use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix};
 use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU};
 
 #[repr(C)]
@@ -372,6 +369,7 @@ impl Multibody {
 
         self.accelerations.fill(0.0);
 
+        // Eqn 42 to 45
         for i in 0..self.links.len() {
             let link = &self.links[i];
             let rb = &bodies[link.rigid_body];
@@ -400,7 +398,7 @@ impl Multibody {
             }
 
             acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23));
-            acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23);
+            acc.linvel += acc.angvel.gcross(link.shift23);
 
             self.workspace.accs[i] = acc;
 
@@ -728,7 +726,7 @@ impl Multibody {
 
     /// The generalized velocity at the multibody_joint of the given link.
     #[inline]
-    pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> {
+    pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> {
         let ndofs = link.joint().ndofs();
         DVectorView::from_slice(
             &self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs],
@@ -829,8 +827,10 @@ impl Multibody {
         }
     }
 
+    // TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians
+    //       (i.e. just something used by the velocity solver’s small steps).
     /// Apply forward-kinematics to this multibody and its related rigid-bodies.
-    pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) {
+    pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) {
         // Special case for the root, which has no parent.
         {
             let link = &mut self.links[0];
@@ -839,7 +839,7 @@ impl Multibody {
 
             if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
                 rb.pos.next_position = link.local_to_world;
-                if update_mass_props {
+                if update_rb_mass_props {
                     rb.mprops.update_world_mass_properties(&link.local_to_world);
                 }
             }
@@ -873,7 +873,7 @@ impl Multibody {
                 "A rigid-body that is not at the root of a multibody must be dynamic."
             );
 
-            if update_mass_props {
+            if update_rb_mass_props {
                 link_rb
                     .mprops
                     .update_world_mass_properties(&link.local_to_world);
@@ -951,40 +951,4 @@ impl Multibody {
             .sum();
         (num_constraints, num_constraints)
     }
-
-    #[inline]
-    pub(crate) fn generate_internal_constraints(
-        &self,
-        params: &IntegrationParameters,
-        j_id: &mut usize,
-        jacobians: &mut DVector<Real>,
-        out: &mut Vec<AnyJointVelocityConstraint>,
-        mut insert_at: Option<usize>,
-    ) {
-        if !cfg!(feature = "parallel") {
-            let num_constraints: usize = self
-                .links
-                .iter()
-                .map(|l| l.joint().num_velocity_constraints())
-                .sum();
-
-            let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2;
-            if jacobians.nrows() < required_jacobian_len {
-                jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
-            }
-        }
-
-        for link in self.links.iter() {
-            link.joint().velocity_constraints(
-                params,
-                self,
-                link,
-                0,
-                j_id,
-                jacobians,
-                out,
-                &mut insert_at,
-            );
-        }
-    }
 }

src/dynamics/joint/multibody_joint/multibody_joint.rs

@@ -1,4 +1,4 @@
-use crate::dynamics::solver::AnyJointVelocityConstraint;
+use crate::dynamics::solver::JointGenericOneBodyConstraint;
 use crate::dynamics::{
     joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
     RigidBodyVelocity,
@@ -254,15 +254,15 @@ impl MultibodyJoint {
         params: &IntegrationParameters,
         multibody: &Multibody,
         link: &MultibodyLink,
-        dof_id: usize,
-        j_id: &mut usize,
+        mut j_id: usize,
         jacobians: &mut DVector<Real>,
-        constraints: &mut Vec<AnyJointVelocityConstraint>,
-        insert_at: &mut Option<usize>,
-    ) {
+        constraints: &mut [JointGenericOneBodyConstraint],
+    ) -> usize {
+        let j_id = &mut j_id;
         let locked_bits = self.data.locked_axes.bits();
         let limit_bits = self.data.limit_axes.bits();
         let motor_bits = self.data.motor_axes.bits();
+        let mut num_constraints = 0;
         let mut curr_free_dof = 0;
 
         for i in 0..DIM {
@@ -281,11 +281,11 @@ impl MultibodyJoint {
                         &self.data.motors[i],
                         self.coords[i],
                         limits,
-                        dof_id + curr_free_dof,
+                        curr_free_dof,
                         j_id,
                         jacobians,
                         constraints,
-                        insert_at,
+                        &mut num_constraints,
                     );
                 }
 
@@ -296,11 +296,11 @@ impl MultibodyJoint {
                         link,
                         [self.data.limits[i].min, self.data.limits[i].max],
                         self.coords[i],
-                        dof_id + curr_free_dof,
+                        curr_free_dof,
                         j_id,
                         jacobians,
                         constraints,
-                        insert_at,
+                        &mut num_constraints,
                     );
                 }
                 curr_free_dof += 1;
@@ -331,11 +331,11 @@ impl MultibodyJoint {
                         link,
                         limits,
                         self.coords[i],
-                        dof_id + curr_free_dof,
+                        curr_free_dof,
                         j_id,
                         jacobians,
                         constraints,
-                        insert_at,
+                        &mut num_constraints,
                     );
                     Some(limits)
                 } else {
@@ -350,15 +350,17 @@ impl MultibodyJoint {
                         &self.data.motors[i],
                         self.coords[i],
                         limits,
-                        dof_id + curr_free_dof,
+                        curr_free_dof,
                         j_id,
                         jacobians,
                         constraints,
-                        insert_at,
+                        &mut num_constraints,
                     );
                 }
                 curr_free_dof += 1;
             }
         }
+
+        num_constraints
     }
 }

src/dynamics/joint/multibody_joint/multibody_joint_set.rs

@@ -53,13 +53,24 @@ impl IndexedData for MultibodyJointHandle {
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
-pub struct MultibodyJointLink {
-    pub graph_id: RigidBodyGraphIndex,
+/// Indexes usable to get a multibody link from a `MultibodyJointSet`.
+///
+/// ```rust
+/// // With:
+/// //     multibody_joint_set: MultibodyJointSet
+/// //     multibody_link_id: MultibodyLinkId
+/// let multibody = &multibody_joint_set[multibody_link_id.multibody];
+/// let link = multibody.link(multibody_link_id.id).expect("Link not found.");
+pub struct MultibodyLinkId {
+    pub(crate) graph_id: RigidBodyGraphIndex,
+    /// The multibody index to be used as `&multibody_joint_set[multibody]` to
+    /// retrieve the multibody reference.
     pub multibody: MultibodyIndex,
+    /// The multibody link index to be given to [`Multibody::link`].
     pub id: usize,
 }
 
-impl Default for MultibodyJointLink {
+impl Default for MultibodyLinkId {
     fn default() -> Self {
         Self {
             graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32),
@@ -78,7 +89,7 @@ impl Default for MultibodyJointLink {
 #[derive(Clone)]
 pub struct MultibodyJointSet {
     pub(crate) multibodies: Arena<Multibody>, // NOTE: a Slab would be sufficient.
-    pub(crate) rb2mb: Coarena<MultibodyJointLink>,
+    pub(crate) rb2mb: Coarena<MultibodyLinkId>,
     // NOTE: this is mostly for the island extraction. So perhaps we won’t need
     //       that any more in the future when we improve our island builder.
     pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
@@ -97,13 +108,22 @@ impl MultibodyJointSet {
     }
 
     /// Iterates through all the multibody joints from this set.
-    pub fn iter(&self) -> impl Iterator<Item = (MultibodyJointHandle, &Multibody, &MultibodyLink)> {
+    pub fn iter(
+        &self,
+    ) -> impl Iterator<
+        Item = (
+            MultibodyJointHandle,
+            &MultibodyLinkId,
+            &Multibody,
+            &MultibodyLink,
+        ),
+    > {
         self.rb2mb
             .iter()
             .filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user.
             .map(|(h, link)| {
                 let mb = &self.multibodies[link.multibody.0];
-                (MultibodyJointHandle(h), mb, mb.link(link.id).unwrap())
+                (MultibodyJointHandle(h), link, mb, mb.link(link.id).unwrap())
             })
     }
 
@@ -118,7 +138,7 @@ impl MultibodyJointSet {
         let data = data.into();
         let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {
             let mb_handle = self.multibodies.insert(Multibody::with_root(body1));
-            MultibodyJointLink {
+            MultibodyLinkId {
                 graph_id: self.connectivity_graph.graph.add_node(body1),
                 multibody: MultibodyIndex(mb_handle),
                 id: 0,
@@ -127,7 +147,7 @@ impl MultibodyJointSet {
 
         let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| {
             let mb_handle = self.multibodies.insert(Multibody::with_root(body2));
-            MultibodyJointLink {
+            MultibodyLinkId {
                 graph_id: self.connectivity_graph.graph.add_node(body2),
                 multibody: MultibodyIndex(mb_handle),
                 id: 0,
@@ -257,7 +277,7 @@ impl MultibodyJointSet {
     /// Returns the link of this multibody attached to the given rigid-body.
     ///
     /// Returns `None` if `rb` isn’t part of any rigid-body.
-    pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyJointLink> {
+    pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> {
         self.rb2mb.get(rb.0)
     }
 
@@ -340,15 +360,15 @@ impl MultibodyJointSet {
         // NOTE: if there is a joint between these two bodies, then
         //       one of the bodies must be the parent of the other.
         let link1 = mb.link(id1.id)?;
-        let parent1 = link1.parent_id()?;
+        let parent1 = link1.parent_id();
 
-        if parent1 == id2.id {
+        if parent1 == Some(id2.id) {
             Some((MultibodyJointHandle(rb1.0), mb, &link1))
         } else {
             let link2 = mb.link(id2.id)?;
-            let parent2 = link2.parent_id()?;
+            let parent2 = link2.parent_id();
 
-            if parent2 == id1.id {
+            if parent2 == Some(id1.id) {
                 Some((MultibodyJointHandle(rb2.0), mb, &link2))
             } else {
                 None

src/dynamics/joint/multibody_joint/unit_multibody_joint.rs

@@ -1,9 +1,7 @@
 #![allow(missing_docs)] // For downcast.
 
 use crate::dynamics::joint::MultibodyLink;
-use crate::dynamics::solver::{
-    AnyJointVelocityConstraint, JointGenericVelocityGroundConstraint, WritebackId,
-};
+use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId};
 use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
 use crate::math::Real;
 use na::DVector;
@@ -19,18 +17,16 @@ pub fn unit_joint_limit_constraint(
     dof_id: usize,
     j_id: &mut usize,
     jacobians: &mut DVector<Real>,
-    constraints: &mut Vec<AnyJointVelocityConstraint>,
-    insert_at: &mut Option<usize>,
+    constraints: &mut [JointGenericOneBodyConstraint],
+    insert_at: &mut usize,
 ) {
     let ndofs = multibody.ndofs();
-    let joint_velocity = multibody.joint_velocity(link);
-
     let min_enabled = curr_pos < limits[0];
     let max_enabled = limits[1] < curr_pos;
     let erp_inv_dt = params.joint_erp_inv_dt();
     let cfm_coeff = params.joint_cfm_coeff();
     let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
-    let rhs_wo_bias = joint_velocity[dof_id];
+    let rhs_wo_bias = 0.0;
 
     let dof_j_id = *j_id + dof_id + link.assembly_id;
     jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
@@ -46,8 +42,8 @@ pub fn unit_joint_limit_constraint(
         max_enabled as u32 as Real * Real::MAX,
     ];
 
-    let constraint = JointGenericVelocityGroundConstraint {
-        mj_lambda2: multibody.solver_id,
+    let constraint = JointGenericOneBodyConstraint {
+        solver_vel2: multibody.solver_id,
         ndofs2: ndofs,
         j_id2: *j_id,
         joint_id: usize::MAX,
@@ -61,14 +57,9 @@ pub fn unit_joint_limit_constraint(
         writeback_id: WritebackId::Limit(dof_id),
     };
 
-    if let Some(at) = insert_at {
-        constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
-        *at += 1;
-    } else {
-        constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
-            constraint,
-        ));
-    }
+    constraints[*insert_at] = constraint;
+    *insert_at += 1;
+
     *j_id += 2 * ndofs;
 }
 
@@ -84,13 +75,11 @@ pub fn unit_joint_motor_constraint(
     dof_id: usize,
     j_id: &mut usize,
     jacobians: &mut DVector<Real>,
-    constraints: &mut Vec<AnyJointVelocityConstraint>,
-    insert_at: &mut Option<usize>,
+    constraints: &mut [JointGenericOneBodyConstraint],
+    insert_at: &mut usize,
 ) {
     let inv_dt = params.inv_dt();
     let ndofs = multibody.ndofs();
-    let joint_velocity = multibody.joint_velocity(link);
-
     let motor_params = motor.motor_params(params.dt);
 
     let dof_j_id = *j_id + dof_id + link.assembly_id;
@@ -117,11 +106,10 @@ pub fn unit_joint_motor_constraint(
         );
     };
 
-    let dvel = joint_velocity[dof_id];
-    rhs_wo_bias += dvel - target_vel;
+    rhs_wo_bias += -target_vel;
 
-    let constraint = JointGenericVelocityGroundConstraint {
-        mj_lambda2: multibody.solver_id,
+    let constraint = JointGenericOneBodyConstraint {
+        solver_vel2: multibody.solver_id,
         ndofs2: ndofs,
         j_id2: *j_id,
         joint_id: usize::MAX,
@@ -135,13 +123,8 @@ pub fn unit_joint_motor_constraint(
         writeback_id: WritebackId::Limit(dof_id),
     };
 
-    if let Some(at) = insert_at {
-        constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
-        *at += 1;
-    } else {
-        constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
-            constraint,
-        ));
-    }
+    constraints[*insert_at] = constraint;
+    *insert_at += 1;
+
     *j_id += 2 * ndofs;
 }

src/dynamics/joint/rope_joint.rs

@@ -1,8 +1,8 @@
 use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
 use crate::dynamics::{JointAxis, MotorModel};
-use crate::math::{Point, Real, UnitVector};
+use crate::math::{Point, Real};
 
-use super::{JointLimits, JointMotor};
+use super::JointMotor;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
@@ -14,12 +14,16 @@ pub struct RopeJoint {
 }
 
 impl RopeJoint {
-    /// Creates a new rope joint limiting the max distance between to bodies
-    pub fn new() -> Self {
-        let data = GenericJointBuilder::new(JointAxesMask::FREE_FIXED_AXES)
+    /// Creates a new rope joint limiting the max distance between two bodies.
+    ///
+    /// The `max_dist` must be strictly greater than 0.0.
+    pub fn new(max_dist: Real) -> Self {
+        let data = GenericJointBuilder::new(JointAxesMask::empty())
             .coupled_axes(JointAxesMask::LIN_AXES)
             .build();
-        Self { data }
+        let mut result = Self { data };
+        result.set_max_distance(max_dist);
+        result
     }
 
     /// The underlying generic joint.
@@ -62,30 +66,6 @@ impl RopeJoint {
         self
     }
 
-    /// The principal axis of the joint, expressed in the local-space of the first rigid-body.
-    #[must_use]
-    pub fn local_axis1(&self) -> UnitVector<Real> {
-        self.data.local_axis1()
-    }
-
-    /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body.
-    pub fn set_local_axis1(&mut self, axis1: UnitVector<Real>) -> &mut Self {
-        self.data.set_local_axis1(axis1);
-        self
-    }
-
-    /// The principal axis of the joint, expressed in the local-space of the second rigid-body.
-    #[must_use]
-    pub fn local_axis2(&self) -> UnitVector<Real> {
-        self.data.local_axis2()
-    }
-
-    /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body.
-    pub fn set_local_axis2(&mut self, axis2: UnitVector<Real>) -> &mut Self {
-        self.data.set_local_axis2(axis2);
-        self
-    }
-
     /// The motor affecting the joint’s translational degree of freedom.
     #[must_use]
     pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
@@ -95,9 +75,6 @@ impl RopeJoint {
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
     pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self {
         self.data.set_motor_model(JointAxis::X, model);
-        self.data.set_motor_model(JointAxis::Y, model);
-        #[cfg(feature = "dim3")]
-        self.data.set_motor_model(JointAxis::Z, model);
         self
     }
 
@@ -105,11 +82,6 @@ impl RopeJoint {
     pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self {
         self.data
             .set_motor_velocity(JointAxis::X, target_vel, factor);
-        self.data
-            .set_motor_velocity(JointAxis::Y, target_vel, factor);
-        #[cfg(feature = "dim3")]
-        self.data
-            .set_motor_velocity(JointAxis::Z, target_vel, factor);
         self
     }
 
@@ -122,11 +94,6 @@ impl RopeJoint {
     ) -> &mut Self {
         self.data
             .set_motor_position(JointAxis::X, target_pos, stiffness, damping);
-        self.data
-            .set_motor_position(JointAxis::Y, target_pos, stiffness, damping);
-        #[cfg(feature = "dim3")]
-        self.data
-            .set_motor_position(JointAxis::Z, target_pos, stiffness, damping);
         self
     }
 
@@ -140,35 +107,26 @@ impl RopeJoint {
     ) -> &mut Self {
         self.data
             .set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping);
-        self.data
-            .set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping);
-        #[cfg(feature = "dim3")]
-        self.data
-            .set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping);
         self
     }
 
     /// Sets the maximum force the motor can deliver.
     pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
         self.data.set_motor_max_force(JointAxis::X, max_force);
-        self.data.set_motor_max_force(JointAxis::Y, max_force);
-        #[cfg(feature = "dim3")]
-        self.data.set_motor_max_force(JointAxis::Z, max_force);
         self
     }
 
-    /// The limit maximum distance attached bodies can translate.
+    /// The maximum distance allowed between the attached objects.
     #[must_use]
-    pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
-        self.data.limits(axis)
+    pub fn max_distance(&self) -> Option<Real> {
+        self.data.limits(JointAxis::X).map(|l| l.max)
     }
 
-    /// Sets the `[min,max]` limit distances attached bodies can translate.
-    pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
-        self.data.set_limits(JointAxis::X, limits);
-        self.data.set_limits(JointAxis::Y, limits);
-        #[cfg(feature = "dim3")]
-        self.data.set_limits(JointAxis::Z, limits);
+    /// Sets the maximum allowed distance between the attached objects.
+    ///
+    /// The `max_dist` must be strictly greater than 0.0.
+    pub fn set_max_distance(&mut self, max_dist: Real) -> &mut Self {
+        self.data.set_limits(JointAxis::X, [0.0, max_dist]);
         self
     }
 }
@@ -190,8 +148,8 @@ impl RopeJointBuilder {
     /// Creates a new builder for rope joints.
     ///
     /// This axis is expressed in the local-space of both rigid-bodies.
-    pub fn new() -> Self {
-        Self(RopeJoint::new())
+    pub fn new(max_dist: Real) -> Self {
+        Self(RopeJoint::new(max_dist))
     }
 
     /// Sets whether contacts between the attached rigid-bodies are enabled.
@@ -215,20 +173,6 @@ impl RopeJointBuilder {
         self
     }
 
-    /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body.
-    #[must_use]
-    pub fn local_axis1(mut self, axis1: UnitVector<Real>) -> Self {
-        self.0.set_local_axis1(axis1);
-        self
-    }
-
-    /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body.
-    #[must_use]
-    pub fn local_axis2(mut self, axis2: UnitVector<Real>) -> Self {
-        self.0.set_local_axis2(axis2);
-        self
-    }
-
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
     #[must_use]
     pub fn motor_model(mut self, model: MotorModel) -> Self {
@@ -270,10 +214,12 @@ impl RopeJointBuilder {
         self
     }
 
-    /// Sets the `[min,max]` limit distances attached bodies can translate.
+    /// Sets the maximum allowed distance between the attached bodies.
+    ///
+    /// The `max_dist` must be strictly greater than 0.0.
     #[must_use]
-    pub fn limits(mut self, limits: [Real; 2]) -> Self {
-        self.0.set_limits(limits);
+    pub fn max_distance(mut self, max_dist: Real) -> Self {
+        self.0.set_max_distance(max_dist);
         self
     }
 

src/dynamics/joint/spring_joint.rs

@@ -0,0 +1,172 @@
+use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
+use crate::dynamics::{JointAxis, MotorModel};
+use crate::math::{Point, Real};
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+#[repr(transparent)]
+/// A spring-damper joint, applies a force proportional to the distance between two objects.
+///
+/// The spring is integrated implicitly, implying that an even undamped spring will still be subject to some
+/// amount of numerical damping (so it will eventually come to a rest). More solver iterations, or smaller
+/// timesteps, will lower the effect of numerical damping, providing a more realistic result.
+pub struct SpringJoint {
+    /// The underlying joint data.
+    pub data: GenericJoint,
+}
+
+impl SpringJoint {
+    /// Creates a new spring joint limiting the max distance between two bodies.
+    ///
+    /// The `max_dist` must be strictly greater than 0.0.
+    pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self {
+        let data = GenericJointBuilder::new(JointAxesMask::empty())
+            .coupled_axes(JointAxesMask::LIN_AXES)
+            .motor_position(JointAxis::X, rest_length, stiffness, damping)
+            .motor_model(JointAxis::X, MotorModel::ForceBased)
+            .build();
+        Self { data }
+    }
+
+    /// The underlying generic joint.
+    pub fn data(&self) -> &GenericJoint {
+        &self.data
+    }
+
+    /// Are contacts between the attached rigid-bodies enabled?
+    pub fn contacts_enabled(&self) -> bool {
+        self.data.contacts_enabled
+    }
+
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
+        self.data.set_contacts_enabled(enabled);
+        self
+    }
+
+    /// The joint’s anchor, expressed in the local-space of the first rigid-body.
+    #[must_use]
+    pub fn local_anchor1(&self) -> Point<Real> {
+        self.data.local_anchor1()
+    }
+
+    /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
+    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor1(anchor1);
+        self
+    }
+
+    /// The joint’s anchor, expressed in the local-space of the second rigid-body.
+    #[must_use]
+    pub fn local_anchor2(&self) -> Point<Real> {
+        self.data.local_anchor2()
+    }
+
+    /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
+    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor2(anchor2);
+        self
+    }
+
+    /// Set the spring model used by this joint to reach the desired target velocity and position.
+    ///
+    /// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants
+    /// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With
+    /// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses,
+    /// making the spring more mass-independent.
+    pub fn set_spring_model(&mut self, model: MotorModel) -> &mut Self {
+        self.data.set_motor_model(JointAxis::X, model);
+        self
+    }
+
+    // /// The maximum distance allowed between the attached objects.
+    // #[must_use]
+    // pub fn rest_length(&self) -> Option<Real> {
+    //     self.data.limits(JointAxis::X).map(|l| l.max)
+    // }
+    //
+    // /// Sets the maximum allowed distance between the attached objects.
+    // ///
+    // /// The `max_dist` must be strictly greater than 0.0.
+    // pub fn set_rest_length(&mut self, max_dist: Real) -> &mut Self {
+    //     self.data.set_limits(JointAxis::X, [0.0, max_dist]);
+    //     self
+    // }
+}
+
+impl Into<GenericJoint> for SpringJoint {
+    fn into(self) -> GenericJoint {
+        self.data
+    }
+}
+
+/// A [SpringJoint] joint using the builder pattern.
+///
+/// This builds a spring-damper joint which applies a force proportional to the distance between two objects.
+/// See the documentation of [SpringJoint] for more information on its behavior.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct SpringJointBuilder(pub SpringJoint);
+
+impl SpringJointBuilder {
+    /// Creates a new builder for spring joints.
+    ///
+    /// This axis is expressed in the local-space of both rigid-bodies.
+    pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self {
+        Self(SpringJoint::new(rest_length, stiffness, damping))
+    }
+
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    #[must_use]
+    pub fn contacts_enabled(mut self, enabled: bool) -> Self {
+        self.0.set_contacts_enabled(enabled);
+        self
+    }
+
+    /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
+    #[must_use]
+    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+        self.0.set_local_anchor1(anchor1);
+        self
+    }
+
+    /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
+    #[must_use]
+    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+        self.0.set_local_anchor2(anchor2);
+        self
+    }
+
+    /// Set the spring used by this joint to reach the desired target velocity and position.
+    ///
+    /// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants
+    /// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With
+    /// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses,
+    /// making the spring more mass-independent.
+    #[must_use]
+    pub fn spring_model(mut self, model: MotorModel) -> Self {
+        self.0.set_spring_model(model);
+        self
+    }
+
+    // /// Sets the maximum allowed distance between the attached bodies.
+    // ///
+    // /// The `max_dist` must be strictly greater than 0.0.
+    // #[must_use]
+    // pub fn max_distance(mut self, max_dist: Real) -> Self {
+    //     self.0.set_max_distance(max_dist);
+    //     self
+    // }
+
+    /// Builds the spring joint.
+    #[must_use]
+    pub fn build(self) -> SpringJoint {
+        self.0
+    }
+}
+
+impl Into<GenericJoint> for SpringJointBuilder {
+    fn into(self) -> GenericJoint {
+        self.0.into()
+    }
+}

src/dynamics/mod.rs

@@ -8,10 +8,10 @@ pub(crate) use self::joint::JointGraphEdge;
 pub(crate) use self::joint::JointIndex;
 pub use self::joint::*;
 pub use self::rigid_body_components::*;
-#[cfg(not(feature = "parallel"))]
+// #[cfg(not(feature = "parallel"))]
 pub(crate) use self::solver::IslandSolver;
-#[cfg(feature = "parallel")]
-pub(crate) use self::solver::ParallelIslandSolver;
+// #[cfg(feature = "parallel")]
+// pub(crate) use self::solver::ParallelIslandSolver;
 pub use parry::mass_properties::MassProperties;
 
 pub use self::rigid_body::{RigidBody, RigidBodyBuilder};

src/dynamics/rigid_body.rs

@@ -7,7 +7,7 @@ use crate::geometry::{
     ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderSet, ColliderShape,
 };
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector};
-use crate::utils::WCross;
+use crate::utils::SimdCross;
 use num::Zero;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -36,6 +36,7 @@ pub struct RigidBody {
     /// The dominance group this rigid-body is part of.
     pub(crate) dominance: RigidBodyDominance,
     pub(crate) enabled: bool,
+    pub(crate) additional_solver_iterations: usize,
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
 }
@@ -64,6 +65,7 @@ impl RigidBody {
             dominance: RigidBodyDominance::default(),
             enabled: true,
             user_data: 0,
+            additional_solver_iterations: 0,
         }
     }
 
@@ -72,6 +74,27 @@ impl RigidBody {
         self.ids = Default::default();
     }
 
+    /// Set the additional number of solver iterations run for this rigid-body and
+    /// everything interacting with it.
+    ///
+    /// See [`Self::set_additional_solver_iterations`] for additional information.
+    pub fn additional_solver_iterations(&self) -> usize {
+        self.additional_solver_iterations
+    }
+
+    /// Set the additional number of solver iterations run for this rigid-body and
+    /// everything interacting with it.
+    ///
+    /// Increasing this number will help improve simulation accuracy on this rigid-body
+    /// and every rigid-body interacting directly or indirectly with it (through joints
+    /// or contacts). This implies a performance hit.
+    ///
+    /// The default value is 0, meaning [`IntegrationParameters::num_solver_iterations`] will
+    /// be used as number of solver iterations for this body.
+    pub fn set_additional_solver_iterations(&mut self, additional_iterations: usize) {
+        self.additional_solver_iterations = additional_iterations;
+    }
+
     /// The activation status of this rigid-body.
     pub fn activation(&self) -> &RigidBodyActivation {
         &self.activation
@@ -1030,6 +1053,11 @@ pub struct RigidBodyBuilder {
     pub enabled: bool,
     /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder.
     pub user_data: u128,
+    /// The additional number of solver iterations run for this rigid-body and
+    /// everything interacting with it.
+    ///
+    /// See [`RigidBody::set_additional_solver_iterations`] for additional information.
+    pub additional_solver_iterations: usize,
 }
 
 impl RigidBodyBuilder {
@@ -1051,6 +1079,7 @@ impl RigidBodyBuilder {
             dominance_group: 0,
             enabled: true,
             user_data: 0,
+            additional_solver_iterations: 0,
         }
     }
 
@@ -1090,6 +1119,15 @@ impl RigidBodyBuilder {
         Self::new(RigidBodyType::Dynamic)
     }
 
+    /// Sets the additional number of solver iterations run for this rigid-body and
+    /// everything interacting with it.
+    ///
+    /// See [`RigidBody::set_additional_solver_iterations`] for additional information.
+    pub fn additional_solver_iterations(mut self, additional_iterations: usize) -> Self {
+        self.additional_solver_iterations = additional_iterations;
+        self
+    }
+
     /// Sets the scale applied to the gravity force affecting the rigid-body to be created.
     pub fn gravity_scale(mut self, scale_factor: Real) -> Self {
         self.gravity_scale = scale_factor;
@@ -1311,6 +1349,7 @@ impl RigidBodyBuilder {
         rb.vels.angvel = self.angvel;
         rb.body_type = self.body_type;
         rb.user_data = self.user_data;
+        rb.additional_solver_iterations = self.additional_solver_iterations;
 
         if self.additional_mass_properties
             != RigidBodyAdditionalMassProps::MassProps(MassProperties::zero())

src/dynamics/rigid_body_components.rs

@@ -7,7 +7,7 @@ use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
 };
 use crate::parry::partitioning::IndexedData;
-use crate::utils::{WAngularInertia, WCross, WDot};
+use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
 use num::Zero;
 
 /// The unique handle of a rigid body added to a `RigidBodySet`.
@@ -307,11 +307,52 @@ impl RigidBodyMassProps {
         self.effective_inv_mass.map(crate::utils::inv)
     }
 
+    /// The square root of the effective world-space angular inertia (that takes the potential rotation locking into account) of
+    /// this rigid-body.
+    #[must_use]
+    pub fn effective_angular_inertia_sqrt(&self) -> AngularInertia<Real> {
+        #[allow(unused_mut)] // mut needed in 3D.
+        let mut ang_inertia = self.effective_world_inv_inertia_sqrt;
+
+        // Make the matrix invertible.
+        #[cfg(feature = "dim3")]
+        {
+            if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
+                ang_inertia.m11 = 1.0;
+            }
+            if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
+                ang_inertia.m22 = 1.0;
+            }
+            if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
+                ang_inertia.m33 = 1.0;
+            }
+        }
+
+        #[allow(unused_mut)] // mut needed in 3D.
+        let mut result = ang_inertia.inverse();
+
+        // Remove the locked axes again.
+        #[cfg(feature = "dim3")]
+        {
+            if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
+                result.m11 = 0.0;
+            }
+            if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
+                result.m22 = 0.0;
+            }
+            if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
+                result.m33 = 0.0;
+            }
+        }
+
+        result
+    }
+
     /// The effective world-space angular inertia (that takes the potential rotation locking into account) of
     /// this rigid-body.
     #[must_use]
     pub fn effective_angular_inertia(&self) -> AngularInertia<Real> {
-        self.effective_world_inv_inertia_sqrt.squared().inverse()
+        self.effective_angular_inertia_sqrt().squared()
     }
 
     /// Recompute the mass-properties of this rigid-bodies based on its currently attached colliders.

src/dynamics/solver/categorization.rs

@@ -6,10 +6,10 @@ pub(crate) fn categorize_contacts(
     multibody_joints: &MultibodyJointSet,
     manifolds: &[&mut ContactManifold],
     manifold_indices: &[ContactManifoldIndex],
-    out_ground: &mut Vec<ContactManifoldIndex>,
-    out_not_ground: &mut Vec<ContactManifoldIndex>,
-    out_generic_ground: &mut Vec<ContactManifoldIndex>,
-    out_generic_not_ground: &mut Vec<ContactManifoldIndex>,
+    out_one_body: &mut Vec<ContactManifoldIndex>,
+    out_two_body: &mut Vec<ContactManifoldIndex>,
+    out_generic_one_body: &mut Vec<ContactManifoldIndex>,
+    out_generic_two_body: &mut Vec<ContactManifoldIndex>,
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
@@ -26,14 +26,14 @@ pub(crate) fn categorize_contacts(
                 .is_some()
         {
             if manifold.data.relative_dominance != 0 {
-                out_generic_ground.push(*manifold_i);
+                out_generic_one_body.push(*manifold_i);
             } else {
-                out_generic_not_ground.push(*manifold_i);
+                out_generic_two_body.push(*manifold_i);
             }
         } else if manifold.data.relative_dominance != 0 {
-            out_ground.push(*manifold_i)
+            out_one_body.push(*manifold_i)
         } else {
-            out_not_ground.push(*manifold_i)
+            out_two_body.push(*manifold_i)
         }
     }
 }
@@ -43,10 +43,10 @@ pub(crate) fn categorize_joints(
     multibody_joints: &MultibodyJointSet,
     impulse_joints: &[JointGraphEdge],
     joint_indices: &[JointIndex],
-    ground_joints: &mut Vec<JointIndex>,
-    nonground_joints: &mut Vec<JointIndex>,
-    generic_ground_joints: &mut Vec<JointIndex>,
-    generic_nonground_joints: &mut Vec<JointIndex>,
+    one_body_joints: &mut Vec<JointIndex>,
+    two_body_joints: &mut Vec<JointIndex>,
+    generic_one_body_joints: &mut Vec<JointIndex>,
+    generic_two_body_joints: &mut Vec<JointIndex>,
 ) {
     for joint_i in joint_indices {
         let joint = &impulse_joints[*joint_i].weight;
@@ -57,14 +57,14 @@ pub(crate) fn categorize_joints(
             || multibody_joints.rigid_body_link(joint.body2).is_some()
         {
             if !rb1.is_dynamic() || !rb2.is_dynamic() {
-                generic_ground_joints.push(*joint_i);
+                generic_one_body_joints.push(*joint_i);
             } else {
-                generic_nonground_joints.push(*joint_i);
+                generic_two_body_joints.push(*joint_i);
             }
         } else if !rb1.is_dynamic() || !rb2.is_dynamic() {
-            ground_joints.push(*joint_i);
+            one_body_joints.push(*joint_i);
         } else {
-            nonground_joints.push(*joint_i);
+            two_body_joints.push(*joint_i);
         }
     }
 }

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
+        );
+    }
+}

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();
+    }
+}

src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs

@@ -1,13 +1,12 @@
 use crate::dynamics::solver::{
-    VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart,
-    VelocityGroundConstraintTangentPart,
+    OneBodyConstraintElement, OneBodyConstraintNormalPart, OneBodyConstraintTangentPart,
 };
 use crate::math::{Real, DIM};
 use na::DVector;
 #[cfg(feature = "dim2")]
 use na::SimdPartialOrd;
 
-impl VelocityGroundConstraintTangentPart<Real> {
+impl OneBodyConstraintTangentPart<Real> {
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -15,21 +14,21 @@ impl VelocityGroundConstraintTangentPart<Real> {
         jacobians: &DVector<Real>,
         ndofs2: usize,
         limit: Real,
-        mj_lambda2: usize,
-        mj_lambdas: &mut DVector<Real>,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
     ) {
         #[cfg(feature = "dim2")]
         {
             let dvel_0 = jacobians
                 .rows(j_id2, ndofs2)
-                .dot(&mj_lambdas.rows(mj_lambda2, 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;
 
-            mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
                 dlambda,
                 &jacobians.rows(j_id2 + ndofs2, ndofs2),
                 1.0,
@@ -41,11 +40,11 @@ impl VelocityGroundConstraintTangentPart<Real> {
             let j_step = ndofs2 * 2;
             let dvel_0 = jacobians
                 .rows(j_id2, ndofs2)
-                .dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+                .dot(&solver_vels.rows(solver_vel2, ndofs2))
                 + self.rhs[0];
             let dvel_1 = jacobians
                 .rows(j_id2 + j_step, ndofs2)
-                .dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+                .dot(&solver_vels.rows(solver_vel2, ndofs2))
                 + self.rhs[1];
 
             let new_impulse = na::Vector2::new(
@@ -57,12 +56,12 @@ impl VelocityGroundConstraintTangentPart<Real> {
             let dlambda = new_impulse - self.impulse;
             self.impulse = new_impulse;
 
-            mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
                 dlambda[0],
                 &jacobians.rows(j_id2 + ndofs2, ndofs2),
                 1.0,
             );
-            mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
                 dlambda[1],
                 &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
                 1.0,
@@ -71,7 +70,7 @@ impl VelocityGroundConstraintTangentPart<Real> {
     }
 }
 
-impl VelocityGroundConstraintNormalPart<Real> {
+impl OneBodyConstraintNormalPart<Real> {
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -79,19 +78,19 @@ impl VelocityGroundConstraintNormalPart<Real> {
         j_id2: usize,
         jacobians: &DVector<Real>,
         ndofs2: usize,
-        mj_lambda2: usize,
-        mj_lambdas: &mut DVector<Real>,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
     ) {
         let dvel = jacobians
             .rows(j_id2, ndofs2)
-            .dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+            .dot(&solver_vels.rows(solver_vel2, ndofs2))
             + self.rhs;
 
         let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
-        mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
+        solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
             dlambda,
             &jacobians.rows(j_id2 + ndofs2, ndofs2),
             1.0,
@@ -99,7 +98,7 @@ impl VelocityGroundConstraintNormalPart<Real> {
     }
 }
 
-impl VelocityGroundConstraintElement<Real> {
+impl OneBodyConstraintElement<Real> {
     #[inline]
     pub fn generic_solve_group(
         cfm_factor: Real,
@@ -109,8 +108,8 @@ impl VelocityGroundConstraintElement<Real> {
         ndofs2: usize,
         // Jacobian index of the first constraint.
         j_id: usize,
-        mj_lambda2: usize,
-        mj_lambdas: &mut DVector<Real>,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
         solve_restitution: bool,
         solve_friction: bool,
     ) {
@@ -122,7 +121,12 @@ impl VelocityGroundConstraintElement<Real> {
 
             for element in elements.iter_mut() {
                 element.normal_part.generic_solve(
-                    cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas,
+                    cfm_factor,
+                    nrm_j_id,
+                    jacobians,
+                    ndofs2,
+                    solver_vel2,
+                    solver_vels,
                 );
                 nrm_j_id += j_step;
             }
@@ -135,7 +139,7 @@ impl VelocityGroundConstraintElement<Real> {
             for element in elements.iter_mut() {
                 let limit = limit * element.normal_part.impulse;
                 let part = &mut element.tangent_part;
-                part.generic_solve(tng_j_id, jacobians, ndofs2, limit, mj_lambda2, mj_lambdas);
+                part.generic_solve(tng_j_id, jacobians, ndofs2, limit, solver_vel2, solver_vels);
                 tng_j_id += j_step;
             }
         }

src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs

@@ -1,43 +1,45 @@
-use crate::dynamics::solver::{GenericRhs, VelocityConstraint};
+use crate::dynamics::solver::{GenericRhs, TwoBodyConstraint};
 use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
-use crate::utils::{WAngularInertia, WCross, WDot};
+use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
 
-use super::{
-    AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
-};
+use super::{TwoBodyConstraintBuilder, TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
+use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::{ContactPointInfos, SolverVel};
+use crate::prelude::RigidBodyHandle;
 #[cfg(feature = "dim2")]
-use crate::utils::WBasis;
+use crate::utils::SimdBasis;
 use na::DVector;
 
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct GenericVelocityConstraint {
-    // We just build the generic constraint on top of the velocity constraint,
-    // adding some information we can use in the generic case.
-    pub velocity_constraint: VelocityConstraint,
-    pub j_id: usize,
-    pub ndofs1: usize,
-    pub ndofs2: usize,
-    pub generic_constraint_mask: u8,
+#[derive(Copy, Clone)]
+pub(crate) struct GenericTwoBodyConstraintBuilder {
+    handle1: RigidBodyHandle,
+    handle2: RigidBodyHandle,
+    ccd_thickness: Real,
+    inner: TwoBodyConstraintBuilder,
 }
 
-impl GenericVelocityConstraint {
+impl GenericTwoBodyConstraintBuilder {
+    pub fn invalid() -> Self {
+        Self {
+            handle1: RigidBodyHandle::invalid(),
+            handle2: RigidBodyHandle::invalid(),
+            ccd_thickness: Real::MAX,
+            inner: TwoBodyConstraintBuilder::invalid(),
+        }
+    }
+
     pub fn generate(
-        params: &IntegrationParameters,
         manifold_id: ContactManifoldIndex,
         manifold: &ContactManifold,
         bodies: &RigidBodySet,
         multibodies: &MultibodyJointSet,
-        out_constraints: &mut Vec<AnyVelocityConstraint>,
+        out_builders: &mut [GenericTwoBodyConstraintBuilder],
+        out_constraints: &mut [GenericTwoBodyConstraint],
         jacobians: &mut DVector<Real>,
         jacobian_id: &mut usize,
-        insert_at: Option<usize>,
     ) {
-        let cfm_factor = params.cfm_factor();
-        let inv_dt = params.inv_dt();
-        let erp_inv_dt = params.erp_inv_dt();
-
         let handle1 = manifold.data.rigid_body1.unwrap();
         let handle2 = manifold.data.rigid_body2.unwrap();
 
@@ -46,7 +48,6 @@ impl GenericVelocityConstraint {
 
         let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type);
         let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type);
-        let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
 
         let multibody1 = multibodies
             .rigid_body_link(handle1)
@@ -54,14 +55,14 @@ impl GenericVelocityConstraint {
         let multibody2 = multibodies
             .rigid_body_link(handle2)
             .map(|m| (&multibodies[m.multibody], m.id));
-        let mj_lambda1 = multibody1
+        let solver_vel1 = multibody1
             .map(|mb| mb.0.solver_id)
             .unwrap_or(if type1.is_dynamic() {
                 rb1.ids.active_set_offset
             } else {
                 0
             });
-        let mj_lambda2 = multibody2
+        let solver_vel2 = multibody2
             .map(|mb| mb.0.solver_id)
             .unwrap_or(if type2.is_dynamic() {
                 rb2.ids.active_set_offset
@@ -87,50 +88,50 @@ impl GenericVelocityConstraint {
             jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
         }
 
-        for (_l, manifold_points) in manifold
+        for (l, manifold_points) in manifold
             .data
             .solver_contacts
             .chunks(MAX_MANIFOLD_POINTS)
             .enumerate()
         {
             let chunk_j_id = *jacobian_id;
-            let mut is_fast_contact = false;
-            let mut constraint = VelocityConstraint {
-                dir1: force_dir1,
-                #[cfg(feature = "dim3")]
-                tangent1: tangents1[0],
-                elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
-                im1: if type1.is_dynamic() {
-                    mprops1.effective_inv_mass
-                } else {
-                    na::zero()
-                },
-                im2: if type2.is_dynamic() {
-                    mprops2.effective_inv_mass
-                } else {
-                    na::zero()
-                },
-                cfm_factor,
-                limit: 0.0,
-                mj_lambda1,
-                mj_lambda2,
-                manifold_id,
-                manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
-                num_contacts: manifold_points.len() as u8,
+
+            let builder = &mut out_builders[l];
+            let constraint = &mut out_constraints[l];
+            constraint.inner.dir1 = force_dir1;
+            constraint.inner.im1 = if type1.is_dynamic() {
+                mprops1.effective_inv_mass
+            } else {
+                na::zero()
             };
+            constraint.inner.im2 = if type2.is_dynamic() {
+                mprops2.effective_inv_mass
+            } else {
+                na::zero()
+            };
+            constraint.inner.solver_vel1 = solver_vel1;
+            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 dp1 = manifold_point.point - mprops1.world_com;
-                let dp2 = manifold_point.point - mprops2.world_com;
+                let point = manifold_point.point;
+                let dp1 = point - mprops1.world_com;
+                let dp2 = point - mprops2.world_com;
 
                 let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
                 let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
 
-                constraint.limit = manifold_point.friction;
-                constraint.manifold_contact_id[k] = manifold_point.contact_id;
+                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_dir1 = dp1.gcross(force_dir1);
                     let torque_dir2 = dp2.gcross(-force_dir1);
@@ -191,23 +192,16 @@ impl GenericVelocityConstraint {
                     let r = crate::utils::inv(inv_r1 + inv_r2);
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
-                    let is_resting = 1.0 - is_bouncy;
 
-                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
-                        * (vel1 - vel2).dot(&force_dir1);
-                    rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting;
-                    let rhs_bias =
-                        /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0);
+                    normal_rhs_wo_bias =
+                        (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1);
 
-                    let rhs = rhs_wo_bias + rhs_bias;
-                    is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5);
-
-                    constraint.elements[k].normal_part = VelocityConstraintNormalPart {
+                    constraint.inner.elements[k].normal_part = TwoBodyConstraintNormalPart {
                         gcross1,
                         gcross2,
-                        rhs,
-                        rhs_wo_bias,
+                        rhs: na::zero(),
+                        rhs_wo_bias: na::zero(),
+                        total_impulse: na::zero(),
                         impulse: na::zero(),
                         r,
                     };
@@ -215,7 +209,7 @@ impl GenericVelocityConstraint {
 
                 // Tangent parts.
                 {
-                    constraint.elements[k].tangent_part.impulse = na::zero();
+                    constraint.inner.elements[k].tangent_part.impulse = na::zero();
 
                     for j in 0..DIM - 1 {
                         let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -226,7 +220,7 @@ impl GenericVelocityConstraint {
                         } else {
                             na::zero()
                         };
-                        constraint.elements[k].tangent_part.gcross1[j] = gcross1;
+                        constraint.inner.elements[k].tangent_part.gcross1[j] = gcross1;
 
                         let torque_dir2 = dp2.gcross(-tangents1[j]);
                         let gcross2 = if type2.is_dynamic() {
@@ -236,7 +230,7 @@ impl GenericVelocityConstraint {
                         } else {
                             na::zero()
                         };
-                        constraint.elements[k].tangent_part.gcross2[j] = gcross2;
+                        constraint.inner.elements[k].tangent_part.gcross2[j] = gcross2;
 
                         let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() {
                             mb1.fill_jacobians(
@@ -277,23 +271,43 @@ impl GenericVelocityConstraint {
                         };
 
                         let r = crate::utils::inv(inv_r1 + inv_r2);
+                        let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]);
 
-                        let rhs =
-                            (vel1 - vel2 + 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;
 
-                        constraint.elements[k].tangent_part.rhs[j] = rhs;
-                        // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
+                        // TODO: 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.elements[k].tangent_part.r[j] = r;
+                        constraint.inner.elements[k].tangent_part.r[j] = r;
                     }
                 }
-            }
 
-            constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
+                // Builder.
+                let infos = ContactPointInfos {
+                    local_p1: rb1
+                        .pos
+                        .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.handle1 = handle1;
+                builder.handle2 = handle2;
+                builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
+                builder.inner.infos[k] = infos;
+                constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
+            }
 
             let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0);
             let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0);
+
             // NOTE: we use the generic constraint for non-dynamic bodies because this will
             //       reduce all ops to nothing because its ndofs will be zero.
             let generic_constraint_mask = (multibody1.is_some() as u8)
@@ -301,78 +315,119 @@ impl GenericVelocityConstraint {
                 | (!type1.is_dynamic() as u8)
                 | ((!type2.is_dynamic() as u8) << 1);
 
-            let constraint = GenericVelocityConstraint {
-                velocity_constraint: constraint,
-                j_id: chunk_j_id,
-                ndofs1,
-                ndofs2,
-                generic_constraint_mask,
-            };
+            constraint.j_id = chunk_j_id;
+            constraint.ndofs1 = ndofs1;
+            constraint.ndofs2 = ndofs2;
+            constraint.generic_constraint_mask = generic_constraint_mask;
+        }
+    }
 
-            if let Some(at) = insert_at {
-                out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGeneric(constraint);
-            } else {
-                out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint));
-            }
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        solved_dt: Real,
+        bodies: &[SolverBody],
+        multibodies: &MultibodyJointSet,
+        constraint: &mut GenericTwoBodyConstraint,
+    ) {
+        // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint.
+        let pos1 = multibodies
+            .rigid_body_link(self.handle1)
+            .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
+            .unwrap_or_else(|| &bodies[constraint.inner.solver_vel1].position);
+        let pos2 = multibodies
+            .rigid_body_link(self.handle2)
+            .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
+            .unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position);
+
+        self.inner.update_with_positions(
+            params,
+            solved_dt,
+            pos1,
+            pos2,
+            self.ccd_thickness,
+            &mut constraint.inner,
+        );
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct GenericTwoBodyConstraint {
+    // We just build the generic constraint on top of the velocity constraint,
+    // adding some information we can use in the generic case.
+    pub inner: TwoBodyConstraint,
+    pub j_id: usize,
+    pub ndofs1: usize,
+    pub ndofs2: usize,
+    pub generic_constraint_mask: u8,
+}
+
+impl GenericTwoBodyConstraint {
+    pub fn invalid() -> Self {
+        Self {
+            inner: TwoBodyConstraint::invalid(),
+            j_id: usize::MAX,
+            ndofs1: usize::MAX,
+            ndofs2: usize::MAX,
+            generic_constraint_mask: u8::MAX,
         }
     }
 
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,
-        mj_lambdas: &mut [DeltaVel<Real>],
-        generic_mj_lambdas: &mut DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
         solve_restitution: bool,
         solve_friction: bool,
     ) {
-        let mut mj_lambda1 = if self.generic_constraint_mask & 0b01 == 0 {
-            GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda1 as usize])
+        let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1 as usize])
         } else {
-            GenericRhs::GenericId(self.velocity_constraint.mj_lambda1 as usize)
+            GenericRhs::GenericId(self.inner.solver_vel1 as usize)
         };
 
-        let mut mj_lambda2 = if self.generic_constraint_mask & 0b10 == 0 {
-            GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda2 as usize])
+        let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2 as usize])
         } else {
-            GenericRhs::GenericId(self.velocity_constraint.mj_lambda2 as usize)
+            GenericRhs::GenericId(self.inner.solver_vel2 as usize)
         };
 
-        let elements = &mut self.velocity_constraint.elements
-            [..self.velocity_constraint.num_contacts as usize];
-        VelocityConstraintElement::generic_solve_group(
-            self.velocity_constraint.cfm_factor,
+        let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
+        TwoBodyConstraintElement::generic_solve_group(
+            self.inner.cfm_factor,
             elements,
             jacobians,
-            &self.velocity_constraint.dir1,
+            &self.inner.dir1,
             #[cfg(feature = "dim3")]
-            &self.velocity_constraint.tangent1,
-            &self.velocity_constraint.im1,
-            &self.velocity_constraint.im2,
-            self.velocity_constraint.limit,
+            &self.inner.tangent1,
+            &self.inner.im1,
+            &self.inner.im2,
+            self.inner.limit,
             self.ndofs1,
             self.ndofs2,
             self.j_id,
-            &mut mj_lambda1,
-            &mut mj_lambda2,
-            generic_mj_lambdas,
+            &mut solver_vel1,
+            &mut solver_vel2,
+            generic_solver_vels,
             solve_restitution,
             solve_friction,
         );
 
-        if let GenericRhs::DeltaVel(mj_lambda1) = mj_lambda1 {
-            mj_lambdas[self.velocity_constraint.mj_lambda1 as usize] = mj_lambda1;
+        if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
+            solver_vels[self.inner.solver_vel1 as usize] = solver_vel1;
         }
 
-        if let GenericRhs::DeltaVel(mj_lambda2) = mj_lambda2 {
-            mj_lambdas[self.velocity_constraint.mj_lambda2 as usize] = mj_lambda2;
+        if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
+            solver_vels[self.inner.solver_vel2 as usize] = solver_vel2;
         }
     }
 
     pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
-        self.velocity_constraint.writeback_impulses(manifolds_all);
+        self.inner.writeback_impulses(manifolds_all);
     }
 
     pub fn remove_cfm_and_bias_from_rhs(&mut self) {
-        self.velocity_constraint.remove_cfm_and_bias_from_rhs();
+        self.inner.remove_cfm_and_bias_from_rhs();
     }
 }

src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs

@@ -1,15 +1,15 @@
-use super::DeltaVel;
+use crate::dynamics::solver::SolverVel;
 use crate::dynamics::solver::{
-    VelocityConstraintElement, VelocityConstraintNormalPart, VelocityConstraintTangentPart,
+    TwoBodyConstraintElement, TwoBodyConstraintNormalPart, TwoBodyConstraintTangentPart,
 };
 use crate::math::{AngVector, Real, Vector, DIM};
-use crate::utils::WDot;
+use crate::utils::SimdDot;
 use na::DVector;
 #[cfg(feature = "dim2")]
-use {crate::utils::WBasis, na::SimdPartialOrd};
+use {crate::utils::SimdBasis, na::SimdPartialOrd};
 
 pub(crate) enum GenericRhs {
-    DeltaVel(DeltaVel<Real>),
+    SolverVel(SolverVel<Real>),
     GenericId(usize),
 }
 
@@ -48,13 +48,13 @@ impl GenericRhs {
         jacobians: &DVector<Real>,
         dir: &Vector<Real>,
         gcross: &AngVector<Real>,
-        mj_lambdas: &DVector<Real>,
+        solver_vels: &DVector<Real>,
     ) -> Real {
         match self {
-            GenericRhs::DeltaVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular),
-            GenericRhs::GenericId(mj_lambda) => {
+            GenericRhs::SolverVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular),
+            GenericRhs::GenericId(solver_vel) => {
                 let j = jacobians.rows(j_id, ndofs);
-                let rhs = mj_lambdas.rows(*mj_lambda, ndofs);
+                let rhs = solver_vels.rows(*solver_vel, ndofs);
                 j.dot(&rhs)
             }
         }
@@ -69,25 +69,25 @@ impl GenericRhs {
         jacobians: &DVector<Real>,
         dir: &Vector<Real>,
         gcross: &AngVector<Real>,
-        mj_lambdas: &mut DVector<Real>,
+        solver_vels: &mut DVector<Real>,
         inv_mass: &Vector<Real>,
     ) {
         match self {
-            GenericRhs::DeltaVel(rhs) => {
+            GenericRhs::SolverVel(rhs) => {
                 rhs.linear += dir.component_mul(inv_mass) * impulse;
                 rhs.angular += gcross * impulse;
             }
-            GenericRhs::GenericId(mj_lambda) => {
+            GenericRhs::GenericId(solver_vel) => {
                 let wj_id = j_id + ndofs;
                 let wj = jacobians.rows(wj_id, ndofs);
-                let mut rhs = mj_lambdas.rows_mut(*mj_lambda, ndofs);
+                let mut rhs = solver_vels.rows_mut(*solver_vel, ndofs);
                 rhs.axpy(impulse, &wj, 1.0);
             }
         }
     }
 }
 
-impl VelocityConstraintTangentPart<Real> {
+impl TwoBodyConstraintTangentPart<Real> {
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -99,9 +99,9 @@ impl VelocityConstraintTangentPart<Real> {
         ndofs1: usize,
         ndofs2: usize,
         limit: Real,
-        mj_lambda1: &mut GenericRhs,
-        mj_lambda2: &mut GenericRhs,
-        mj_lambdas: &mut DVector<Real>,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
     ) {
         let j_id1 = j_id1(j_id, ndofs1, ndofs2);
         let j_id2 = j_id2(j_id, ndofs1, ndofs2);
@@ -110,79 +110,79 @@ impl VelocityConstraintTangentPart<Real> {
 
         #[cfg(feature = "dim2")]
         {
-            let dvel_0 = mj_lambda1.dvel(
+            let dvel_0 = solver_vel1.dvel(
                 j_id1,
                 ndofs1,
                 jacobians,
                 &tangents1[0],
                 &self.gcross1[0],
-                mj_lambdas,
-            ) + mj_lambda2.dvel(
+                solver_vels,
+            ) + solver_vel2.dvel(
                 j_id2,
                 ndofs2,
                 jacobians,
                 &-tangents1[0],
                 &self.gcross2[0],
-                mj_lambdas,
+                solver_vels,
             ) + 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;
 
-            mj_lambda1.apply_impulse(
+            solver_vel1.apply_impulse(
                 j_id1,
                 ndofs1,
                 dlambda,
                 jacobians,
                 &tangents1[0],
                 &self.gcross1[0],
-                mj_lambdas,
+                solver_vels,
                 im1,
             );
-            mj_lambda2.apply_impulse(
+            solver_vel2.apply_impulse(
                 j_id2,
                 ndofs2,
                 dlambda,
                 jacobians,
                 &-tangents1[0],
                 &self.gcross2[0],
-                mj_lambdas,
+                solver_vels,
                 im2,
             );
         }
 
         #[cfg(feature = "dim3")]
         {
-            let dvel_0 = mj_lambda1.dvel(
+            let dvel_0 = solver_vel1.dvel(
                 j_id1,
                 ndofs1,
                 jacobians,
                 &tangents1[0],
                 &self.gcross1[0],
-                mj_lambdas,
-            ) + mj_lambda2.dvel(
+                solver_vels,
+            ) + solver_vel2.dvel(
                 j_id2,
                 ndofs2,
                 jacobians,
                 &-tangents1[0],
                 &self.gcross2[0],
-                mj_lambdas,
+                solver_vels,
             ) + self.rhs[0];
-            let dvel_1 = mj_lambda1.dvel(
+            let dvel_1 = solver_vel1.dvel(
                 j_id1 + j_step,
                 ndofs1,
                 jacobians,
                 &tangents1[1],
                 &self.gcross1[1],
-                mj_lambdas,
-            ) + mj_lambda2.dvel(
+                solver_vels,
+            ) + solver_vel2.dvel(
                 j_id2 + j_step,
                 ndofs2,
                 jacobians,
                 &-tangents1[1],
                 &self.gcross2[1],
-                mj_lambdas,
+                solver_vels,
             ) + self.rhs[1];
 
             let new_impulse = na::Vector2::new(
@@ -194,52 +194,52 @@ impl VelocityConstraintTangentPart<Real> {
             let dlambda = new_impulse - self.impulse;
             self.impulse = new_impulse;
 
-            mj_lambda1.apply_impulse(
+            solver_vel1.apply_impulse(
                 j_id1,
                 ndofs1,
                 dlambda[0],
                 jacobians,
                 &tangents1[0],
                 &self.gcross1[0],
-                mj_lambdas,
+                solver_vels,
                 im1,
             );
-            mj_lambda1.apply_impulse(
+            solver_vel1.apply_impulse(
                 j_id1 + j_step,
                 ndofs1,
                 dlambda[1],
                 jacobians,
                 &tangents1[1],
                 &self.gcross1[1],
-                mj_lambdas,
+                solver_vels,
                 im1,
             );
 
-            mj_lambda2.apply_impulse(
+            solver_vel2.apply_impulse(
                 j_id2,
                 ndofs2,
                 dlambda[0],
                 jacobians,
                 &-tangents1[0],
                 &self.gcross2[0],
-                mj_lambdas,
+                solver_vels,
                 im2,
             );
-            mj_lambda2.apply_impulse(
+            solver_vel2.apply_impulse(
                 j_id2 + j_step,
                 ndofs2,
                 dlambda[1],
                 jacobians,
                 &-tangents1[1],
                 &self.gcross2[1],
-                mj_lambdas,
+                solver_vels,
                 im2,
             );
         }
     }
 }
 
-impl VelocityConstraintNormalPart<Real> {
+impl TwoBodyConstraintNormalPart<Real> {
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -251,45 +251,45 @@ impl VelocityConstraintNormalPart<Real> {
         im2: &Vector<Real>,
         ndofs1: usize,
         ndofs2: usize,
-        mj_lambda1: &mut GenericRhs,
-        mj_lambda2: &mut GenericRhs,
-        mj_lambdas: &mut DVector<Real>,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
     ) {
         let j_id1 = j_id1(j_id, ndofs1, ndofs2);
         let j_id2 = j_id2(j_id, ndofs1, ndofs2);
 
-        let dvel = mj_lambda1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
-            + mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
+        let dvel = solver_vel1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, solver_vels)
+            + solver_vel2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, solver_vels)
             + self.rhs;
 
         let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
-        mj_lambda1.apply_impulse(
+        solver_vel1.apply_impulse(
             j_id1,
             ndofs1,
             dlambda,
             jacobians,
             &dir1,
             &self.gcross1,
-            mj_lambdas,
+            solver_vels,
             im1,
         );
-        mj_lambda2.apply_impulse(
+        solver_vel2.apply_impulse(
             j_id2,
             ndofs2,
             dlambda,
             jacobians,
             &-dir1,
             &self.gcross2,
-            mj_lambdas,
+            solver_vels,
             im2,
         );
     }
 }
 
-impl VelocityConstraintElement<Real> {
+impl TwoBodyConstraintElement<Real> {
     #[inline]
     pub fn generic_solve_group(
         cfm_factor: Real,
@@ -306,9 +306,9 @@ impl VelocityConstraintElement<Real> {
         ndofs2: usize,
         // Jacobian index of the first constraint.
         j_id: usize,
-        mj_lambda1: &mut GenericRhs,
-        mj_lambda2: &mut GenericRhs,
-        mj_lambdas: &mut DVector<Real>,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
         solve_restitution: bool,
         solve_friction: bool,
     ) {
@@ -320,8 +320,17 @@ impl VelocityConstraintElement<Real> {
 
             for element in elements.iter_mut() {
                 element.normal_part.generic_solve(
-                    cfm_factor, nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1,
-                    mj_lambda2, mj_lambdas,
+                    cfm_factor,
+                    nrm_j_id,
+                    jacobians,
+                    &dir1,
+                    im1,
+                    im2,
+                    ndofs1,
+                    ndofs2,
+                    solver_vel1,
+                    solver_vel2,
+                    solver_vels,
                 );
                 nrm_j_id += j_step;
             }
@@ -339,8 +348,17 @@ impl VelocityConstraintElement<Real> {
                 let limit = limit * element.normal_part.impulse;
                 let part = &mut element.tangent_part;
                 part.generic_solve(
-                    tng_j_id, jacobians, tangents1, im1, im2, ndofs1, ndofs2, limit, mj_lambda1,
-                    mj_lambda2, mj_lambdas,
+                    tng_j_id,
+                    jacobians,
+                    tangents1,
+                    im1,
+                    im2,
+                    ndofs1,
+                    ndofs2,
+                    limit,
+                    solver_vel1,
+                    solver_vel2,
+                    solver_vels,
                 );
                 tng_j_id += j_step;
             }

src/dynamics/solver/contact_constraint/mod.rs

@@ -0,0 +1,29 @@
+pub(crate) use generic_one_body_constraint::*;
+// pub(crate) use generic_one_body_constraint_element::*;
+pub(crate) use contact_constraints_set::{
+    ConstraintsCounts, ContactConstraintTypes, ContactConstraintsSet,
+};
+pub(crate) use generic_two_body_constraint::*;
+pub(crate) use generic_two_body_constraint_element::*;
+pub(crate) use one_body_constraint::*;
+pub(crate) use one_body_constraint_element::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(crate) use one_body_constraint_simd::*;
+pub(crate) use two_body_constraint::*;
+pub(crate) use two_body_constraint_element::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(crate) use two_body_constraint_simd::*;
+
+mod contact_constraints_set;
+mod generic_one_body_constraint;
+mod generic_one_body_constraint_element;
+mod generic_two_body_constraint;
+mod generic_two_body_constraint_element;
+mod one_body_constraint;
+mod one_body_constraint_element;
+#[cfg(feature = "simd-is-enabled")]
+mod one_body_constraint_simd;
+mod two_body_constraint;
+mod two_body_constraint_element;
+#[cfg(feature = "simd-is-enabled")]
+mod two_body_constraint_simd;

src/dynamics/solver/contact_constraint/one_body_constraint.rs

@@ -0,0 +1,382 @@
+use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
+use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
+#[cfg(feature = "dim2")]
+use crate::utils::SimdBasis;
+use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
+use parry::math::Isometry;
+
+use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::SolverVel;
+use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+
+// TODO: move this struct somewhere else.
+#[derive(Copy, Clone, Debug)]
+pub struct ContactPointInfos<N: SimdRealCopy> {
+    pub tangent_vel: Vector<N>,
+    pub local_p1: Point<N>,
+    pub local_p2: Point<N>,
+    pub dist: N,
+    pub normal_rhs_wo_bias: N,
+}
+
+impl<N: SimdRealCopy> Default for ContactPointInfos<N> {
+    fn default() -> Self {
+        Self {
+            tangent_vel: Vector::zeros(),
+            local_p1: Point::origin(),
+            local_p2: Point::origin(),
+            dist: N::zero(),
+            normal_rhs_wo_bias: N::zero(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct OneBodyConstraintBuilder {
+    // PERF: only store what’s necessary for the bias updates instead of the complete solver body.
+    pub rb1: SolverBody,
+    pub vels1: RigidBodyVelocity,
+    pub infos: [ContactPointInfos<Real>; MAX_MANIFOLD_POINTS],
+}
+
+impl OneBodyConstraintBuilder {
+    pub fn invalid() -> Self {
+        Self {
+            rb1: SolverBody::default(),
+            vels1: RigidBodyVelocity::zero(),
+            infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS],
+        }
+    }
+
+    pub fn generate(
+        manifold_id: ContactManifoldIndex,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_builders: &mut [OneBodyConstraintBuilder],
+        out_constraints: &mut [OneBodyConstraint],
+    ) {
+        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 = &rb2.vels;
+        let mprops2 = &rb2.mprops;
+
+        #[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 solver_vel2 = rb2.ids.active_set_offset;
+
+        for (l, manifold_points) in manifold
+            .data
+            .solver_contacts
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            let builder = &mut out_builders[l];
+            let constraint = &mut out_constraints[l];
+
+            builder.rb1 = rb1;
+            builder.vels1 = vels1;
+
+            constraint.dir1 = force_dir1;
+            constraint.im2 = mprops2.effective_inv_mass;
+            constraint.solver_vel2 = solver_vel2;
+            constraint.manifold_id = manifold_id;
+            constraint.num_contacts = manifold_points.len() as u8;
+            #[cfg(feature = "dim3")]
+            {
+                constraint.tangent1 = tangents1[0];
+            }
+
+            for k in 0..manifold_points.len() {
+                let manifold_point = &manifold_points[k];
+
+                let dp2 = manifold_point.point - mprops2.world_com;
+                let dp1 = manifold_point.point - world_com1;
+                let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
+                let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
+
+                constraint.limit = manifold_point.friction;
+                constraint.manifold_contact_id[k] = manifold_point.contact_id;
+
+                // Normal part.
+                let normal_rhs_wo_bias;
+                {
+                    let gcross2 = mprops2
+                        .effective_world_inv_inertia_sqrt
+                        .transform_vector(dp2.gcross(-force_dir1));
+
+                    let projected_mass = utils::inv(
+                        force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+                            + gcross2.gdot(gcross2),
+                    );
+
+                    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.elements[k].normal_part = OneBodyConstraintNormalPart {
+                        gcross2,
+                        rhs: na::zero(),
+                        rhs_wo_bias: na::zero(),
+                        impulse: na::zero(),
+                        total_impulse: na::zero(),
+                        r: projected_mass,
+                    };
+                }
+
+                // Tangent parts.
+                {
+                    constraint.elements[k].tangent_part.impulse = na::zero();
+
+                    for j in 0..DIM - 1 {
+                        let gcross2 = mprops2
+                            .effective_world_inv_inertia_sqrt
+                            .transform_vector(dp2.gcross(-tangents1[j]));
+                        let r = tangents1[j]
+                            .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+                            + gcross2.gdot(gcross2);
+                        let rhs_wo_bias = (vel1
+                            + flipped_multiplier * manifold_point.tangent_velocity)
+                            .dot(&tangents1[j]);
+
+                        constraint.elements[k].tangent_part.gcross2[j] = gcross2;
+                        constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
+                        constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
+                        constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
+                            utils::inv(r)
+                        } else {
+                            r
+                        };
+                    }
+
+                    #[cfg(feature = "dim3")]
+                    {
+                        constraint.elements[k].tangent_part.r[2] = 2.0
+                            * constraint.elements[k].tangent_part.gcross2[0]
+                                .gdot(constraint.elements[k].tangent_part.gcross2[1]);
+                    }
+                }
+
+                // Builder.
+                {
+                    let local_p1 = rb1.position.inverse_transform_point(&manifold_point.point);
+                    let local_p2 = rb2
+                        .pos
+                        .position
+                        .inverse_transform_point(&manifold_point.point);
+                    let infos = ContactPointInfos {
+                        local_p1,
+                        local_p2,
+                        tangent_vel: flipped_multiplier * manifold_point.tangent_velocity,
+                        dist: manifold_point.dist,
+                        normal_rhs_wo_bias,
+                    };
+
+                    builder.infos[k] = infos;
+                }
+            }
+        }
+    }
+
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        solved_dt: Real,
+        bodies: &[SolverBody],
+        _multibodies: &MultibodyJointSet,
+        constraint: &mut OneBodyConstraint,
+    ) {
+        let rb2 = &bodies[constraint.solver_vel2];
+        self.update_with_positions(
+            params,
+            solved_dt,
+            &rb2.position,
+            rb2.ccd_thickness,
+            constraint,
+        )
+    }
+
+    // TODO: this code is SOOOO similar to TwoBodyConstraint::update.
+    //       In fact the only differences are types and the `rb1` and ignoring its ccd thickness.
+    pub fn update_with_positions(
+        &self,
+        params: &IntegrationParameters,
+        solved_dt: Real,
+        rb2_pos: &Isometry<Real>,
+        ccd_thickness: Real,
+        constraint: &mut OneBodyConstraint,
+    ) {
+        let cfm_factor = params.cfm_factor();
+        let inv_dt = params.inv_dt();
+        let erp_inv_dt = params.erp_inv_dt();
+
+        let all_infos = &self.infos[..constraint.num_contacts as usize];
+        let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
+        let rb1 = &self.rb1;
+        // Integrate the velocity of the static rigid-body, if it’s kinematic.
+        let new_pos1 = self
+            .vels1
+            .integrate(solved_dt, &rb1.position, &rb1.local_com);
+        let mut is_fast_contact = false;
+
+        #[cfg(feature = "dim2")]
+        let tangents1 = constraint.dir1.orthonormal_basis();
+        #[cfg(feature = "dim3")]
+        let tangents1 = [
+            constraint.tangent1,
+            constraint.dir1.cross(&constraint.tangent1),
+        ];
+
+        for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
+            // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
+            let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt;
+            let p2 = rb2_pos * info.local_p2;
+            let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
+
+            // Normal part.
+            {
+                let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
+                let rhs_bias = erp_inv_dt
+                    * (dist + params.allowed_linear_error)
+                        .clamp(-params.max_penetration_correction, 0.0);
+                let new_rhs = rhs_wo_bias + rhs_bias;
+                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
+                is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
+
+                element.normal_part.rhs_wo_bias = rhs_wo_bias;
+                element.normal_part.rhs = new_rhs;
+                element.normal_part.total_impulse = total_impulse;
+                element.normal_part.impulse = na::zero();
+            }
+
+            // Tangent part.
+            {
+                element.tangent_part.total_impulse += element.tangent_part.impulse;
+                element.tangent_part.impulse = na::zero();
+
+                for j in 0..DIM - 1 {
+                    let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
+                    element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
+                }
+            }
+        }
+
+        constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct OneBodyConstraint {
+    pub solver_vel2: usize,
+    pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
+    #[cfg(feature = "dim3")]
+    pub tangent1: Vector<Real>, // One of the friction force directions.
+    pub im2: Vector<Real>,
+    pub cfm_factor: Real,
+    pub limit: Real,
+    pub elements: [OneBodyConstraintElement<Real>; MAX_MANIFOLD_POINTS],
+
+    pub manifold_id: ContactManifoldIndex,
+    pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
+    pub num_contacts: u8,
+}
+
+impl OneBodyConstraint {
+    pub fn invalid() -> Self {
+        Self {
+            solver_vel2: usize::MAX,
+            dir1: Vector::zeros(),
+            #[cfg(feature = "dim3")]
+            tangent1: Vector::zeros(),
+            im2: Vector::zeros(),
+            cfm_factor: 0.0,
+            limit: 0.0,
+            elements: [OneBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+            manifold_id: ContactManifoldIndex::MAX,
+            manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS],
+            num_contacts: u8::MAX,
+        }
+    }
+
+    pub fn solve(
+        &mut self,
+        solver_vels: &mut [SolverVel<Real>],
+        solve_normal: bool,
+        solve_friction: bool,
+    ) {
+        let mut solver_vel2 = solver_vels[self.solver_vel2 as usize];
+
+        OneBodyConstraintElement::solve_group(
+            self.cfm_factor,
+            &mut self.elements[..self.num_contacts as usize],
+            &self.dir1,
+            #[cfg(feature = "dim3")]
+            &self.tangent1,
+            &self.im2,
+            self.limit,
+            &mut solver_vel2,
+            solve_normal,
+            solve_friction,
+        );
+
+        solver_vels[self.solver_vel2 as usize] = solver_vel2;
+    }
+
+    // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        let manifold = &mut manifolds_all[self.manifold_id];
+
+        for k in 0..self.num_contacts as usize {
+            let contact_id = self.manifold_contact_id[k];
+            let active_contact = &mut manifold.points[contact_id as usize];
+            active_contact.data.impulse = self.elements[k].normal_part.impulse;
+
+            #[cfg(feature = "dim2")]
+            {
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
+            }
+            #[cfg(feature = "dim3")]
+            {
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
+            }
+        }
+    }
+
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.cfm_factor = 1.0;
+        for elt in &mut self.elements {
+            elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
+            elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
+        }
+    }
+}

src/dynamics/solver/contact_constraint/one_body_constraint_element.rs

@@ -0,0 +1,232 @@
+use crate::dynamics::solver::SolverVel;
+use crate::math::{AngVector, Vector, DIM};
+use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
+    pub gcross2: [AngVector<N>; DIM - 1],
+    pub rhs: [N; DIM - 1],
+    pub rhs_wo_bias: [N; DIM - 1],
+    #[cfg(feature = "dim2")]
+    pub impulse: na::Vector1<N>,
+    #[cfg(feature = "dim3")]
+    pub impulse: na::Vector2<N>,
+    #[cfg(feature = "dim2")]
+    pub total_impulse: na::Vector1<N>,
+    #[cfg(feature = "dim3")]
+    pub total_impulse: na::Vector2<N>,
+    #[cfg(feature = "dim2")]
+    pub r: [N; 1],
+    #[cfg(feature = "dim3")]
+    pub r: [N; DIM],
+}
+
+impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
+    fn zero() -> Self {
+        Self {
+            gcross2: [na::zero(); DIM - 1],
+            rhs: [na::zero(); DIM - 1],
+            rhs_wo_bias: [na::zero(); DIM - 1],
+            impulse: na::zero(),
+            total_impulse: na::zero(),
+            #[cfg(feature = "dim2")]
+            r: [na::zero(); 1],
+            #[cfg(feature = "dim3")]
+            r: [na::zero(); DIM],
+        }
+    }
+
+    #[inline]
+    pub fn apply_limit(
+        &mut self,
+        tangents1: [&Vector<N>; DIM - 1],
+        im2: &Vector<N>,
+        limit: N,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        #[cfg(feature = "dim2")]
+        {
+            let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
+            let dlambda = new_impulse - self.impulse[0];
+            self.impulse[0] = new_impulse;
+
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
+            solver_vel2.angular += self.gcross2[0] * dlambda;
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let new_impulse = self.impulse;
+            let new_impulse = {
+                let _disable_fe_except =
+                    crate::utils::DisableFloatingPointExceptionsFlags::
+                    disable_floating_point_exceptions();
+                new_impulse.simd_cap_magnitude(limit)
+            };
+            let dlambda = new_impulse - self.impulse;
+            self.impulse = new_impulse;
+
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+                + tangents1[1].component_mul(im2) * -dlambda[1];
+            solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
+        }
+    }
+
+    #[inline]
+    pub fn solve(
+        &mut self,
+        tangents1: [&Vector<N>; DIM - 1],
+        im2: &Vector<N>,
+        limit: N,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        #[cfg(feature = "dim2")]
+        {
+            let dvel = -tangents1[0].dot(&solver_vel2.linear)
+                + self.gcross2[0].gdot(solver_vel2.angular)
+                + self.rhs[0];
+            let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
+            let dlambda = new_impulse - self.impulse[0];
+            self.impulse[0] = new_impulse;
+
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
+            solver_vel2.angular += self.gcross2[0] * dlambda;
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let dvel_0 = -tangents1[0].dot(&solver_vel2.linear)
+                + self.gcross2[0].gdot(solver_vel2.angular)
+                + self.rhs[0];
+            let dvel_1 = -tangents1[1].dot(&solver_vel2.linear)
+                + self.gcross2[1].gdot(solver_vel2.angular)
+                + self.rhs[1];
+
+            let dvel_00 = dvel_0 * dvel_0;
+            let dvel_11 = dvel_1 * dvel_1;
+            let dvel_01 = dvel_0 * dvel_1;
+            let inv_lhs = (dvel_00 + dvel_11)
+                * crate::utils::simd_inv(
+                    dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
+                );
+            let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
+            let new_impulse = self.impulse - delta_impulse;
+            let new_impulse = {
+                let _disable_fe_except =
+                    crate::utils::DisableFloatingPointExceptionsFlags::
+                    disable_floating_point_exceptions();
+                new_impulse.simd_cap_magnitude(limit)
+            };
+            let dlambda = new_impulse - self.impulse;
+            self.impulse = new_impulse;
+
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+                + tangents1[1].component_mul(im2) * -dlambda[1];
+            solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct OneBodyConstraintNormalPart<N: SimdRealCopy> {
+    pub gcross2: AngVector<N>,
+    pub rhs: N,
+    pub rhs_wo_bias: N,
+    pub impulse: N,
+    pub total_impulse: N,
+    pub r: N,
+}
+
+impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
+    fn zero() -> Self {
+        Self {
+            gcross2: na::zero(),
+            rhs: na::zero(),
+            rhs_wo_bias: na::zero(),
+            impulse: na::zero(),
+            total_impulse: na::zero(),
+            r: na::zero(),
+        }
+    }
+
+    #[inline]
+    pub fn solve(
+        &mut self,
+        cfm_factor: N,
+        dir1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        let dvel =
+            -dir1.dot(&solver_vel2.linear) + self.gcross2.gdot(solver_vel2.angular) + self.rhs;
+        let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
+        let dlambda = new_impulse - self.impulse;
+        self.impulse = new_impulse;
+
+        solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
+        solver_vel2.angular += self.gcross2 * dlambda;
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct OneBodyConstraintElement<N: SimdRealCopy> {
+    pub normal_part: OneBodyConstraintNormalPart<N>,
+    pub tangent_part: OneBodyConstraintTangentPart<N>,
+}
+
+impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
+    pub fn zero() -> Self {
+        Self {
+            normal_part: OneBodyConstraintNormalPart::zero(),
+            tangent_part: OneBodyConstraintTangentPart::zero(),
+        }
+    }
+
+    #[inline]
+    pub fn solve_group(
+        cfm_factor: N,
+        elements: &mut [Self],
+        dir1: &Vector<N>,
+        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
+        im2: &Vector<N>,
+        limit: N,
+        solver_vel2: &mut SolverVel<N>,
+        solve_normal: bool,
+        solve_friction: bool,
+    ) where
+        Vector<N>: SimdBasis,
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        #[cfg(feature = "dim3")]
+        let tangents1 = [tangent1, &dir1.cross(&tangent1)];
+        #[cfg(feature = "dim2")]
+        let tangents1 = [&dir1.orthonormal_vector()];
+
+        // Solve penetration.
+        if solve_normal {
+            for element in elements.iter_mut() {
+                element
+                    .normal_part
+                    .solve(cfm_factor, &dir1, im2, solver_vel2);
+                let limit = limit * element.normal_part.impulse;
+                let part = &mut element.tangent_part;
+                part.apply_limit(tangents1, im2, limit, solver_vel2);
+            }
+        }
+
+        // Solve friction.
+        if solve_friction {
+            for element in elements.iter_mut() {
+                let limit = limit * element.normal_part.impulse;
+                let part = &mut element.tangent_part;
+                part.solve(tangents1, im2, limit, solver_vel2);
+            }
+        }
+    }
+}

src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs

@@ -1,52 +1,38 @@
-use super::{
-    AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
-    VelocityGroundConstraintNormalPart,
-};
+use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
+use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::{ContactPointInfos, SolverVel};
 use crate::dynamics::{
-    IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
+    IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
+    RigidBodyVelocity,
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
-    AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
+    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
+    SIMD_WIDTH,
 };
 #[cfg(feature = "dim2")]
-use crate::utils::WBasis;
-use crate::utils::{self, WAngularInertia, WCross, WDot};
+use crate::utils::SimdBasis;
+use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
 use num::Zero;
 use parry::math::SimdBool;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
-pub(crate) struct WVelocityGroundConstraint {
-    pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
-    #[cfg(feature = "dim3")]
-    pub tangent1: Vector<SimdReal>, // One of the friction force directions.
-    pub elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
-    pub num_contacts: u8,
-    pub im2: Vector<SimdReal>,
-    pub cfm_factor: SimdReal,
-    pub limit: SimdReal,
-    pub mj_lambda2: [usize; SIMD_WIDTH],
-    pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
-    pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
+pub(crate) struct SimdOneBodyConstraintBuilder {
+    // PERF: only store what’s needed, and store it in simd form.
+    rb1: [SolverBody; SIMD_WIDTH],
+    vels1: [RigidBodyVelocity; SIMD_WIDTH],
+    infos: [ContactPointInfos<SimdReal>; MAX_MANIFOLD_POINTS],
 }
 
-impl WVelocityGroundConstraint {
+impl SimdOneBodyConstraintBuilder {
     pub fn generate(
-        params: &IntegrationParameters,
         manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
         manifolds: [&ContactManifold; SIMD_WIDTH],
         bodies: &RigidBodySet,
-        out_constraints: &mut Vec<AnyVelocityConstraint>,
-        insert_at: Option<usize>,
+        out_builders: &mut [SimdOneBodyConstraintBuilder],
+        out_constraints: &mut [OneBodyConstraintSimd],
     ) {
-        let cfm_factor = SimdReal::splat(params.cfm_factor());
-        let dt = SimdReal::splat(params.dt);
-        let inv_dt = SimdReal::splat(params.inv_dt());
-        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
-        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
-        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
-
         let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
         let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
         let mut flipped = [1.0; SIMD_WIDTH];
@@ -58,23 +44,26 @@ impl WVelocityGroundConstraint {
             }
         }
 
+        let rb1: [SolverBody; SIMD_WIDTH] = gather![|ii| {
+            handles1[ii]
+                .map(|h| SolverBody::from(&bodies[h]))
+                .unwrap_or_else(SolverBody::default)
+        }];
+
         let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| {
             handles1[ii]
                 .map(|h| bodies[h].vels)
-                .unwrap_or_else(RigidBodyVelocity::zero)
+                .unwrap_or_else(RigidBodyVelocity::default)
         }];
-        let world_com1 = Point::from(gather![|ii| {
-            handles1[ii]
-                .map(|h| bodies[h].mprops.world_com)
-                .unwrap_or_else(Point::origin)
-        }]);
+
+        let world_com1 = Point::from(gather![|ii| { rb1[ii].world_com }]);
+        let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
 
         let bodies2 = gather![|ii| &bodies[handles2[ii].unwrap()]];
 
         let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies2[ii].vels];
         let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies2[ii].ids];
         let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies2[ii].mprops];
-        let ccd_thickness = SimdReal::from(gather![|ii| bodies2[ii].ccd.ccd_thickness]);
 
         let flipped_sign = SimdReal::from(flipped);
 
@@ -88,12 +77,13 @@ impl WVelocityGroundConstraint {
         let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
         let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
 
+        let poss2 = Isometry::from(gather![|ii| bodies2[ii].pos.position]);
         let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
 
         let normal1 = Vector::from(gather![|ii| manifolds[ii].data.normal]);
         let force_dir1 = normal1 * -flipped_sign;
 
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
+        let solver_vel2 = gather![|ii| ids2[ii].active_set_offset];
 
         let num_active_contacts = manifolds[0].data.num_active_contacts();
 
@@ -106,20 +96,21 @@ impl WVelocityGroundConstraint {
             let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
-            let mut is_fast_contact = SimdBool::splat(false);
-            let mut constraint = WVelocityGroundConstraint {
-                dir1: force_dir1,
-                #[cfg(feature = "dim3")]
-                tangent1: tangents1[0],
-                elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
-                im2,
-                cfm_factor,
-                limit: SimdReal::splat(0.0),
-                mj_lambda2,
-                manifold_id,
-                manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
-                num_contacts: num_points as u8,
-            };
+            let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
+            let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
+
+            builder.rb1 = rb1;
+            builder.vels1 = vels1;
+
+            constraint.dir1 = force_dir1;
+            constraint.im2 = im2;
+            constraint.solver_vel2 = solver_vel2;
+            constraint.manifold_id = manifold_id;
+            constraint.num_contacts = num_points as u8;
+            #[cfg(feature = "dim3")]
+            {
+                constraint.tangent1 = tangents1[0];
+            }
 
             for k in 0..num_points {
                 let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
@@ -127,9 +118,10 @@ impl WVelocityGroundConstraint {
                 let is_bouncy = SimdReal::from(gather![
                     |ii| manifold_points[ii][k].is_bouncy() as u32 as Real
                 ]);
-                let is_resting = SimdReal::splat(1.0) - is_bouncy;
-                let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
+
                 let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
+                let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
+
                 let tangent_velocity =
                     Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
 
@@ -143,6 +135,7 @@ impl WVelocityGroundConstraint {
                 constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
 
                 // Normal part.
+                let normal_rhs_wo_bias;
                 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
@@ -150,24 +143,18 @@ impl WVelocityGroundConstraint {
                         force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2),
                     );
 
-                    let projected_velocity = (vel1 - vel2).dot(&force_dir1);
-                    let mut rhs_wo_bias =
-                        (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
-                    rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting;
-                    let rhs_bias = (dist + allowed_lin_err)
-                        .simd_clamp(-max_penetration_correction, SimdReal::zero())
-                        * (erp_inv_dt/* * is_resting */);
+                    let projected_vel1 = vel1.dot(&force_dir1);
+                    let projected_vel2 = vel2.dot(&force_dir1);
+                    let projected_velocity = projected_vel1 - projected_vel2;
+                    normal_rhs_wo_bias =
+                        (is_bouncy * restitution) * projected_velocity + projected_vel1; // Add projected_vel1 since it’s not accessible through solver_vel.
 
-                    let rhs = rhs_wo_bias + rhs_bias;
-                    is_fast_contact =
-                        is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
-
-                    constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
+                    constraint.elements[k].normal_part = OneBodyConstraintNormalPart {
                         gcross2,
-                        rhs,
-                        rhs_wo_bias,
+                        rhs: na::zero(),
+                        rhs_wo_bias: na::zero(),
                         impulse: na::zero(),
+                        total_impulse: na::zero(),
                         r: projected_mass,
                     };
                 }
@@ -179,10 +166,11 @@ impl WVelocityGroundConstraint {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
                     let r =
                         tangents1[j].dot(&im2.component_mul(&tangents1[j])) + gcross2.gdot(gcross2);
-                    let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
+                    let rhs_wo_bias = (vel1 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
 
                     constraint.elements[k].tangent_part.gcross2[j] = gcross2;
-                    constraint.elements[k].tangent_part.rhs[j] = rhs;
+                    constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
+                    constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
                     constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
                         utils::simd_inv(r)
                     } else {
@@ -196,33 +184,139 @@ impl WVelocityGroundConstraint {
                         * constraint.elements[k].tangent_part.gcross2[0]
                             .gdot(constraint.elements[k].tangent_part.gcross2[1]);
                 }
-            }
 
-            constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+                // Builder.
+                {
+                    let local_p1 = poss1.inverse_transform_point(&point);
+                    let local_p2 = poss2.inverse_transform_point(&point);
+                    let infos = ContactPointInfos {
+                        local_p1,
+                        local_p2,
+                        tangent_vel: tangent_velocity * flipped_sign,
+                        dist,
+                        normal_rhs_wo_bias,
+                    };
 
-            if let Some(at) = insert_at {
-                out_constraints[at + l / MAX_MANIFOLD_POINTS] =
-                    AnyVelocityConstraint::GroupedGround(constraint);
-            } else {
-                out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
+                    builder.infos[k] = infos;
+                }
             }
         }
     }
 
+    // TODO: this code is SOOOO similar to TwoBodyConstraintSimd::update.
+    //       In fact the only differences are types and the `rb1` and ignoring its ccd thickness.
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        solved_dt: Real,
+        bodies: &[SolverBody],
+        _multibodies: &MultibodyJointSet,
+        constraint: &mut OneBodyConstraintSimd,
+    ) {
+        let cfm_factor = SimdReal::splat(params.cfm_factor());
+        let dt = SimdReal::splat(params.dt);
+        let inv_dt = SimdReal::splat(params.inv_dt());
+        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
+        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
+        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
+
+        let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
+        let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
+        let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
+
+        let all_infos = &self.infos[..constraint.num_contacts as usize];
+        let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
+
+        // Integrate the velocity of the static rigid-body, if it’s kinematic.
+        let new_pos1 = Isometry::from(gather![|ii| self.vels1[ii].integrate(
+            solved_dt,
+            &self.rb1[ii].position,
+            &self.rb1[ii].local_com
+        )]);
+
+        #[cfg(feature = "dim2")]
+        let tangents1 = constraint.dir1.orthonormal_basis();
+        #[cfg(feature = "dim3")]
+        let tangents1 = [
+            constraint.tangent1,
+            constraint.dir1.cross(&constraint.tangent1),
+        ];
+
+        let mut is_fast_contact = SimdBool::splat(false);
+        let solved_dt = SimdReal::splat(solved_dt);
+
+        for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
+            // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
+            let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt;
+            let p2 = poss2 * info.local_p2;
+            let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
+
+            // Normal part.
+            {
+                let rhs_wo_bias =
+                    info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
+                let rhs_bias = (dist + allowed_lin_err)
+                    .simd_clamp(-max_penetration_correction, SimdReal::zero())
+                    * erp_inv_dt;
+                let new_rhs = rhs_wo_bias + rhs_bias;
+                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
+                is_fast_contact =
+                    is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
+
+                element.normal_part.rhs_wo_bias = rhs_wo_bias;
+                element.normal_part.rhs = new_rhs;
+                element.normal_part.total_impulse = total_impulse;
+                element.normal_part.impulse = na::zero();
+            }
+
+            // tangent parts.
+            {
+                element.tangent_part.total_impulse += element.tangent_part.impulse;
+                element.tangent_part.impulse = na::zero();
+
+                for j in 0..DIM - 1 {
+                    let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
+                    element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
+                }
+            }
+        }
+
+        constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct OneBodyConstraintSimd {
+    pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
+    #[cfg(feature = "dim3")]
+    pub tangent1: Vector<SimdReal>, // One of the friction force directions.
+    pub elements: [OneBodyConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
+    pub num_contacts: u8,
+    pub im2: Vector<SimdReal>,
+    pub cfm_factor: SimdReal,
+    pub limit: SimdReal,
+    pub solver_vel2: [usize; SIMD_WIDTH],
+    pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+    pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
+}
+
+impl OneBodyConstraintSimd {
     pub fn solve(
         &mut self,
-        mj_lambdas: &mut [DeltaVel<Real>],
+        solver_vels: &mut [SolverVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
     ) {
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
+        let mut solver_vel2 = SolverVel {
+            linear: Vector::from(gather![
+                |ii| solver_vels[self.solver_vel2[ii] as usize].linear
+            ]),
             angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
+                |ii| solver_vels[self.solver_vel2[ii] as usize].angular
             ]),
         };
 
-        VelocityGroundConstraintElement::solve_group(
+        OneBodyConstraintElement::solve_group(
             self.cfm_factor,
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
@@ -230,18 +324,18 @@ impl WVelocityGroundConstraint {
             &self.tangent1,
             &self.im2,
             self.limit,
-            &mut mj_lambda2,
+            &mut solver_vel2,
             solve_normal,
             solve_friction,
         );
 
         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);
+            solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii);
+            solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii);
         }
     }
 
-    // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
+    // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
     pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
         for k in 0..self.num_contacts as usize {
             let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
@@ -272,6 +366,7 @@ impl WVelocityGroundConstraint {
         self.cfm_factor = SimdReal::splat(1.0);
         for elt in &mut self.elements {
             elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
+            elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
         }
     }
 }

src/dynamics/solver/contact_constraint/two_body_constraint.rs

@@ -0,0 +1,470 @@
+use super::{ContactConstraintTypes, ContactPointInfos};
+use crate::dynamics::solver::SolverVel;
+use crate::dynamics::solver::{AnyConstraintMut, SolverBody};
+
+use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{Isometry, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
+use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot};
+use na::DVector;
+
+use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
+
+impl<'a> AnyConstraintMut<'a, ContactConstraintTypes> {
+    pub fn remove_bias(&mut self) {
+        match self {
+            Self::OneBody(c) => c.remove_cfm_and_bias_from_rhs(),
+            Self::TwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
+            Self::GenericOneBody(c) => c.remove_cfm_and_bias_from_rhs(),
+            Self::GenericTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.remove_cfm_and_bias_from_rhs(),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
+        }
+    }
+
+    pub fn solve_restitution(
+        &mut self,
+        generic_jacobians: &DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        match self {
+            Self::OneBody(c) => c.solve(solver_vels, true, false),
+            Self::TwoBodies(c) => c.solve(solver_vels, true, false),
+            Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, true, false),
+            Self::GenericTwoBodies(c) => c.solve(
+                generic_jacobians,
+                solver_vels,
+                generic_solver_vels,
+                true,
+                false,
+            ),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.solve(solver_vels, true, false),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.solve(solver_vels, true, false),
+        }
+    }
+
+    pub fn solve_friction(
+        &mut self,
+        generic_jacobians: &DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        match self {
+            Self::OneBody(c) => c.solve(solver_vels, false, true),
+            Self::TwoBodies(c) => c.solve(solver_vels, false, true),
+            Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, false, true),
+            Self::GenericTwoBodies(c) => c.solve(
+                generic_jacobians,
+                solver_vels,
+                generic_solver_vels,
+                false,
+                true,
+            ),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.solve(solver_vels, false, true),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.solve(solver_vels, false, true),
+        }
+    }
+
+    pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) {
+        match self {
+            Self::OneBody(c) => c.writeback_impulses(manifolds_all),
+            Self::TwoBodies(c) => c.writeback_impulses(manifolds_all),
+            Self::GenericOneBody(c) => c.writeback_impulses(manifolds_all),
+            Self::GenericTwoBodies(c) => c.writeback_impulses(manifolds_all),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.writeback_impulses(manifolds_all),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.writeback_impulses(manifolds_all),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct TwoBodyConstraint {
+    pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
+    #[cfg(feature = "dim3")]
+    pub tangent1: Vector<Real>, // One of the friction force directions.
+    pub im1: Vector<Real>,
+    pub im2: Vector<Real>,
+    pub cfm_factor: Real,
+    pub limit: Real,
+    pub solver_vel1: usize,
+    pub solver_vel2: usize,
+    pub manifold_id: ContactManifoldIndex,
+    pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
+    pub num_contacts: u8,
+    pub elements: [TwoBodyConstraintElement<Real>; MAX_MANIFOLD_POINTS],
+}
+
+impl TwoBodyConstraint {
+    pub fn invalid() -> Self {
+        Self {
+            dir1: Vector::zeros(),
+            #[cfg(feature = "dim3")]
+            tangent1: Vector::zeros(),
+            im1: Vector::zeros(),
+            im2: Vector::zeros(),
+            cfm_factor: 0.0,
+            limit: 0.0,
+            solver_vel1: usize::MAX,
+            solver_vel2: usize::MAX,
+            manifold_id: ContactManifoldIndex::MAX,
+            manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS],
+            num_contacts: u8::MAX,
+            elements: [TwoBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct TwoBodyConstraintBuilder {
+    pub infos: [ContactPointInfos<Real>; MAX_MANIFOLD_POINTS],
+}
+
+impl TwoBodyConstraintBuilder {
+    pub fn invalid() -> Self {
+        Self {
+            infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS],
+        }
+    }
+
+    pub fn generate(
+        manifold_id: ContactManifoldIndex,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_builders: &mut [TwoBodyConstraintBuilder],
+        out_constraints: &mut [TwoBodyConstraint],
+    ) {
+        assert_eq!(manifold.data.relative_dominance, 0);
+
+        let handle1 = manifold.data.rigid_body1.unwrap();
+        let handle2 = manifold.data.rigid_body2.unwrap();
+
+        let rb1 = &bodies[handle1];
+        let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
+        let rb2 = &bodies[handle2];
+        let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
+
+        let solver_vel1 = rb1.ids.active_set_offset;
+        let solver_vel2 = rb2.ids.active_set_offset;
+        let force_dir1 = -manifold.data.normal;
+
+        #[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);
+
+        for (l, manifold_points) in manifold
+            .data
+            .solver_contacts
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            let builder = &mut out_builders[l];
+            let constraint = &mut out_constraints[l];
+            constraint.dir1 = force_dir1;
+            constraint.im1 = mprops1.effective_inv_mass;
+            constraint.im2 = mprops2.effective_inv_mass;
+            constraint.solver_vel1 = solver_vel1;
+            constraint.solver_vel2 = solver_vel2;
+            constraint.manifold_id = manifold_id;
+            constraint.num_contacts = manifold_points.len() as u8;
+            #[cfg(feature = "dim3")]
+            {
+                constraint.tangent1 = tangents1[0];
+            }
+
+            for k in 0..manifold_points.len() {
+                let manifold_point = &manifold_points[k];
+                let point = manifold_point.point;
+
+                let dp1 = point - mprops1.world_com;
+                let dp2 = point - mprops2.world_com;
+
+                let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
+                let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
+
+                constraint.limit = manifold_point.friction;
+                constraint.manifold_contact_id[k] = manifold_point.contact_id;
+
+                // Normal part.
+                let normal_rhs_wo_bias;
+                {
+                    let gcross1 = mprops1
+                        .effective_world_inv_inertia_sqrt
+                        .transform_vector(dp1.gcross(force_dir1));
+                    let gcross2 = mprops2
+                        .effective_world_inv_inertia_sqrt
+                        .transform_vector(dp2.gcross(-force_dir1));
+
+                    let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
+                    let projected_mass = utils::inv(
+                        force_dir1.dot(&imsum.component_mul(&force_dir1))
+                            + gcross1.gdot(gcross1)
+                            + gcross2.gdot(gcross2),
+                    );
+
+                    let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
+
+                    normal_rhs_wo_bias =
+                        (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1);
+
+                    constraint.elements[k].normal_part = TwoBodyConstraintNormalPart {
+                        gcross1,
+                        gcross2,
+                        rhs: na::zero(),
+                        rhs_wo_bias: na::zero(),
+                        impulse: na::zero(),
+                        total_impulse: na::zero(),
+                        r: projected_mass,
+                    };
+                }
+
+                // Tangent parts.
+                {
+                    constraint.elements[k].tangent_part.impulse = na::zero();
+
+                    for j in 0..DIM - 1 {
+                        let gcross1 = mprops1
+                            .effective_world_inv_inertia_sqrt
+                            .transform_vector(dp1.gcross(tangents1[j]));
+                        let gcross2 = mprops2
+                            .effective_world_inv_inertia_sqrt
+                            .transform_vector(dp2.gcross(-tangents1[j]));
+                        let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
+                        let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+                            + gcross1.gdot(gcross1)
+                            + gcross2.gdot(gcross2);
+                        let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]);
+
+                        constraint.elements[k].tangent_part.gcross1[j] = gcross1;
+                        constraint.elements[k].tangent_part.gcross2[j] = gcross2;
+                        constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
+                        constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
+                        constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
+                            utils::inv(r)
+                        } else {
+                            r
+                        };
+                    }
+
+                    #[cfg(feature = "dim3")]
+                    {
+                        constraint.elements[k].tangent_part.r[2] = 2.0
+                            * (constraint.elements[k].tangent_part.gcross1[0]
+                                .gdot(constraint.elements[k].tangent_part.gcross1[1])
+                                + constraint.elements[k].tangent_part.gcross2[0]
+                                    .gdot(constraint.elements[k].tangent_part.gcross2[1]));
+                    }
+                }
+
+                // Builder.
+                let infos = ContactPointInfos {
+                    local_p1: rb1
+                        .pos
+                        .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.infos[k] = infos;
+                constraint.manifold_contact_id[k] = manifold_point.contact_id;
+            }
+        }
+    }
+
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        solved_dt: Real,
+        bodies: &[SolverBody],
+        _multibodies: &MultibodyJointSet,
+        constraint: &mut TwoBodyConstraint,
+    ) {
+        let rb1 = &bodies[constraint.solver_vel1];
+        let rb2 = &bodies[constraint.solver_vel2];
+        let ccd_thickness = rb1.ccd_thickness + rb2.ccd_thickness;
+        self.update_with_positions(
+            params,
+            solved_dt,
+            &rb1.position,
+            &rb2.position,
+            ccd_thickness,
+            constraint,
+        )
+    }
+
+    // Used by both generic and non-generic builders..
+    pub fn update_with_positions(
+        &self,
+        params: &IntegrationParameters,
+        solved_dt: Real,
+        rb1_pos: &Isometry<Real>,
+        rb2_pos: &Isometry<Real>,
+        ccd_thickness: Real,
+        constraint: &mut TwoBodyConstraint,
+    ) {
+        let cfm_factor = params.cfm_factor();
+        let inv_dt = params.inv_dt();
+        let erp_inv_dt = params.erp_inv_dt();
+
+        let all_infos = &self.infos[..constraint.num_contacts as usize];
+        let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
+
+        let mut is_fast_contact = false;
+
+        #[cfg(feature = "dim2")]
+        let tangents1 = constraint.dir1.orthonormal_basis();
+        #[cfg(feature = "dim3")]
+        let tangents1 = [
+            constraint.tangent1,
+            constraint.dir1.cross(&constraint.tangent1),
+        ];
+
+        for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
+            // Tangent velocity is equivalent to the first body’s surface moving artificially.
+            let p1 = rb1_pos * info.local_p1 + info.tangent_vel * solved_dt;
+            let p2 = rb2_pos * info.local_p2;
+            let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
+
+            // Normal part.
+            {
+                let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
+                let rhs_bias = erp_inv_dt
+                    * (dist + params.allowed_linear_error)
+                        .clamp(-params.max_penetration_correction, 0.0);
+                let new_rhs = rhs_wo_bias + rhs_bias;
+                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
+                is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
+
+                element.normal_part.rhs_wo_bias = rhs_wo_bias;
+                element.normal_part.rhs = new_rhs;
+                element.normal_part.total_impulse = total_impulse;
+                element.normal_part.impulse = na::zero();
+            }
+
+            // Tangent part.
+            {
+                element.tangent_part.total_impulse += element.tangent_part.impulse;
+                element.tangent_part.impulse = na::zero();
+
+                for j in 0..DIM - 1 {
+                    let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
+                    element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
+                }
+            }
+        }
+
+        constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
+    }
+}
+
+impl TwoBodyConstraint {
+    pub fn solve(
+        &mut self,
+        solver_vels: &mut [SolverVel<Real>],
+        solve_normal: bool,
+        solve_friction: bool,
+    ) {
+        let mut solver_vel1 = solver_vels[self.solver_vel1 as usize];
+        let mut solver_vel2 = solver_vels[self.solver_vel2 as usize];
+
+        TwoBodyConstraintElement::solve_group(
+            self.cfm_factor,
+            &mut self.elements[..self.num_contacts as usize],
+            &self.dir1,
+            #[cfg(feature = "dim3")]
+            &self.tangent1,
+            &self.im1,
+            &self.im2,
+            self.limit,
+            &mut solver_vel1,
+            &mut solver_vel2,
+            solve_normal,
+            solve_friction,
+        );
+
+        solver_vels[self.solver_vel1 as usize] = solver_vel1;
+        solver_vels[self.solver_vel2 as usize] = solver_vel2;
+    }
+
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        let manifold = &mut manifolds_all[self.manifold_id];
+
+        for k in 0..self.num_contacts as usize {
+            let contact_id = self.manifold_contact_id[k];
+            let active_contact = &mut manifold.points[contact_id as usize];
+            active_contact.data.impulse = self.elements[k].normal_part.impulse;
+
+            #[cfg(feature = "dim2")]
+            {
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
+            }
+            #[cfg(feature = "dim3")]
+            {
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
+            }
+        }
+    }
+
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.cfm_factor = 1.0;
+        for elt in &mut self.elements {
+            elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
+            // elt.normal_part.impulse = elt.normal_part.total_impulse;
+
+            elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
+            // elt.tangent_part.impulse = elt.tangent_part.total_impulse;
+        }
+    }
+}
+
+#[inline(always)]
+#[cfg(feature = "dim3")]
+pub(crate) fn compute_tangent_contact_directions<N>(
+    force_dir1: &Vector<N>,
+    linvel1: &Vector<N>,
+    linvel2: &Vector<N>,
+) -> [Vector<N>; DIM - 1]
+where
+    N: utils::SimdRealCopy,
+    Vector<N>: SimdBasis,
+{
+    use na::SimdValue;
+
+    // Compute the tangent direction. Pick the direction of
+    // the linear relative velocity, if it is not too small.
+    // Otherwise use a fallback direction.
+    let relative_linvel = linvel1 - linvel2;
+    let mut tangent_relative_linvel =
+        relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel));
+
+    let tangent_linvel_norm = {
+        let _disable_fe_except =
+            crate::utils::DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions();
+        tangent_relative_linvel.normalize_mut()
+    };
+
+    const THRESHOLD: Real = 1.0e-4;
+    let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD));
+    let tangent_fallback = force_dir1.orthonormal_vector();
+
+    let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel);
+    let bitangent1 = force_dir1.cross(&tangent1);
+
+    [tangent1, bitangent1]
+}

src/dynamics/solver/contact_constraint/two_body_constraint_element.rs

@@ -0,0 +1,271 @@
+use crate::dynamics::solver::SolverVel;
+use crate::math::{AngVector, Vector, DIM};
+use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
+    pub gcross1: [AngVector<N>; DIM - 1],
+    pub gcross2: [AngVector<N>; DIM - 1],
+    pub rhs: [N; DIM - 1],
+    pub rhs_wo_bias: [N; DIM - 1],
+    #[cfg(feature = "dim2")]
+    pub impulse: na::Vector1<N>,
+    #[cfg(feature = "dim3")]
+    pub impulse: na::Vector2<N>,
+    #[cfg(feature = "dim2")]
+    pub total_impulse: na::Vector1<N>,
+    #[cfg(feature = "dim3")]
+    pub total_impulse: na::Vector2<N>,
+    #[cfg(feature = "dim2")]
+    pub r: [N; 1],
+    #[cfg(feature = "dim3")]
+    pub r: [N; DIM],
+}
+
+impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
+    fn zero() -> Self {
+        Self {
+            gcross1: [na::zero(); DIM - 1],
+            gcross2: [na::zero(); DIM - 1],
+            rhs: [na::zero(); DIM - 1],
+            rhs_wo_bias: [na::zero(); DIM - 1],
+            impulse: na::zero(),
+            total_impulse: na::zero(),
+            #[cfg(feature = "dim2")]
+            r: [na::zero(); 1],
+            #[cfg(feature = "dim3")]
+            r: [na::zero(); DIM],
+        }
+    }
+
+    #[inline]
+    pub fn apply_limit(
+        &mut self,
+        tangents1: [&Vector<N>; DIM - 1],
+        im1: &Vector<N>,
+        im2: &Vector<N>,
+        limit: N,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        #[cfg(feature = "dim2")]
+        {
+            let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
+            let dlambda = new_impulse - self.impulse[0];
+            self.impulse[0] = new_impulse;
+
+            solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda;
+            solver_vel1.angular += self.gcross1[0] * dlambda;
+
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
+            solver_vel2.angular += self.gcross2[0] * dlambda;
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let new_impulse = self.impulse;
+            let new_impulse = {
+                let _disable_fe_except =
+                    crate::utils::DisableFloatingPointExceptionsFlags::
+                    disable_floating_point_exceptions();
+                new_impulse.simd_cap_magnitude(limit)
+            };
+
+            let dlambda = new_impulse - self.impulse;
+            self.impulse = new_impulse;
+
+            solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+                + tangents1[1].component_mul(im1) * dlambda[1];
+            solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
+
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+                + tangents1[1].component_mul(im2) * -dlambda[1];
+            solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
+        }
+    }
+
+    #[inline]
+    pub fn solve(
+        &mut self,
+        tangents1: [&Vector<N>; DIM - 1],
+        im1: &Vector<N>,
+        im2: &Vector<N>,
+        limit: N,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        #[cfg(feature = "dim2")]
+        {
+            let dvel = tangents1[0].dot(&solver_vel1.linear)
+                + self.gcross1[0].gdot(solver_vel1.angular)
+                - tangents1[0].dot(&solver_vel2.linear)
+                + self.gcross2[0].gdot(solver_vel2.angular)
+                + self.rhs[0];
+            let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
+            let dlambda = new_impulse - self.impulse[0];
+            self.impulse[0] = new_impulse;
+
+            solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda;
+            solver_vel1.angular += self.gcross1[0] * dlambda;
+
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
+            solver_vel2.angular += self.gcross2[0] * dlambda;
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let dvel_0 = tangents1[0].dot(&solver_vel1.linear)
+                + self.gcross1[0].gdot(solver_vel1.angular)
+                - tangents1[0].dot(&solver_vel2.linear)
+                + self.gcross2[0].gdot(solver_vel2.angular)
+                + self.rhs[0];
+            let dvel_1 = tangents1[1].dot(&solver_vel1.linear)
+                + self.gcross1[1].gdot(solver_vel1.angular)
+                - tangents1[1].dot(&solver_vel2.linear)
+                + self.gcross2[1].gdot(solver_vel2.angular)
+                + self.rhs[1];
+
+            let dvel_00 = dvel_0 * dvel_0;
+            let dvel_11 = dvel_1 * dvel_1;
+            let dvel_01 = dvel_0 * dvel_1;
+            let inv_lhs = (dvel_00 + dvel_11)
+                * crate::utils::simd_inv(
+                    dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
+                );
+            let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
+            let new_impulse = self.impulse - delta_impulse;
+            let new_impulse = {
+                let _disable_fe_except =
+                        crate::utils::DisableFloatingPointExceptionsFlags::
+                        disable_floating_point_exceptions();
+                new_impulse.simd_cap_magnitude(limit)
+            };
+
+            let dlambda = new_impulse - self.impulse;
+            self.impulse = new_impulse;
+
+            solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+                + tangents1[1].component_mul(im1) * dlambda[1];
+            solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
+
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+                + tangents1[1].component_mul(im2) * -dlambda[1];
+            solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct TwoBodyConstraintNormalPart<N: SimdRealCopy> {
+    pub gcross1: AngVector<N>,
+    pub gcross2: AngVector<N>,
+    pub rhs: N,
+    pub rhs_wo_bias: N,
+    pub impulse: N,
+    pub total_impulse: N,
+    pub r: N,
+}
+
+impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
+    fn zero() -> Self {
+        Self {
+            gcross1: na::zero(),
+            gcross2: na::zero(),
+            rhs: na::zero(),
+            rhs_wo_bias: na::zero(),
+            impulse: na::zero(),
+            total_impulse: na::zero(),
+            r: na::zero(),
+        }
+    }
+
+    #[inline]
+    pub fn solve(
+        &mut self,
+        cfm_factor: N,
+        dir1: &Vector<N>,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        let dvel = dir1.dot(&solver_vel1.linear) + self.gcross1.gdot(solver_vel1.angular)
+            - dir1.dot(&solver_vel2.linear)
+            + self.gcross2.gdot(solver_vel2.angular)
+            + self.rhs;
+        let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
+        let dlambda = new_impulse - self.impulse;
+        self.impulse = new_impulse;
+
+        solver_vel1.linear += dir1.component_mul(im1) * dlambda;
+        solver_vel1.angular += self.gcross1 * dlambda;
+
+        solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
+        solver_vel2.angular += self.gcross2 * dlambda;
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct TwoBodyConstraintElement<N: SimdRealCopy> {
+    pub normal_part: TwoBodyConstraintNormalPart<N>,
+    pub tangent_part: TwoBodyConstraintTangentPart<N>,
+}
+
+impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
+    pub fn zero() -> Self {
+        Self {
+            normal_part: TwoBodyConstraintNormalPart::zero(),
+            tangent_part: TwoBodyConstraintTangentPart::zero(),
+        }
+    }
+
+    #[inline]
+    pub fn solve_group(
+        cfm_factor: N,
+        elements: &mut [Self],
+        dir1: &Vector<N>,
+        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
+        limit: N,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+        solve_normal: bool,
+        solve_friction: bool,
+    ) where
+        Vector<N>: SimdBasis,
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        #[cfg(feature = "dim3")]
+        let tangents1 = [tangent1, &dir1.cross(&tangent1)];
+        #[cfg(feature = "dim2")]
+        let tangents1 = [&dir1.orthonormal_vector()];
+
+        // Solve penetration.
+        if solve_normal {
+            for element in elements.iter_mut() {
+                element
+                    .normal_part
+                    .solve(cfm_factor, &dir1, im1, im2, solver_vel1, solver_vel2);
+                let limit = limit * element.normal_part.impulse;
+                let part = &mut element.tangent_part;
+                part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
+            }
+        }
+
+        // Solve friction.
+        if solve_friction {
+            for element in elements.iter_mut() {
+                let limit = limit * element.normal_part.impulse;
+                let part = &mut element.tangent_part;
+                part.solve(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
+            }
+        }
+    }
+}

src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs

@@ -1,57 +1,39 @@
-use super::{
-    AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
-};
+use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
+use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::{ContactPointInfos, SolverVel};
 use crate::dynamics::{
-    IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
+    IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
+    RigidBodyVelocity,
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
-    AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
+    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
+    SIMD_WIDTH,
 };
 #[cfg(feature = "dim2")]
-use crate::utils::WBasis;
-use crate::utils::{self, WAngularInertia, WCross, WDot};
+use crate::utils::SimdBasis;
+use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
 use num::Zero;
 use parry::math::SimdBool;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
-pub(crate) struct WVelocityConstraint {
-    pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
-    #[cfg(feature = "dim3")]
-    pub tangent1: Vector<SimdReal>, // One of the friction force directions.
-    pub elements: [VelocityConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
-    pub num_contacts: u8,
-    pub im1: Vector<SimdReal>,
-    pub im2: Vector<SimdReal>,
-    pub cfm_factor: SimdReal,
-    pub limit: SimdReal,
-    pub mj_lambda1: [usize; SIMD_WIDTH],
-    pub mj_lambda2: [usize; SIMD_WIDTH],
-    pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
-    pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
+pub(crate) struct TwoBodyConstraintBuilderSimd {
+    infos: [super::ContactPointInfos<SimdReal>; MAX_MANIFOLD_POINTS],
 }
 
-impl WVelocityConstraint {
+impl TwoBodyConstraintBuilderSimd {
     pub fn generate(
-        params: &IntegrationParameters,
         manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
         manifolds: [&ContactManifold; SIMD_WIDTH],
         bodies: &RigidBodySet,
-        out_constraints: &mut Vec<AnyVelocityConstraint>,
-        insert_at: Option<usize>,
+        out_builders: &mut [TwoBodyConstraintBuilderSimd],
+        out_constraints: &mut [TwoBodyConstraintSimd],
     ) {
         for ii in 0..SIMD_WIDTH {
             assert_eq!(manifolds[ii].data.relative_dominance, 0);
         }
 
-        let cfm_factor = SimdReal::splat(params.cfm_factor());
-        let dt = SimdReal::splat(params.dt);
-        let inv_dt = SimdReal::splat(params.inv_dt());
-        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
-        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
-        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
-
         let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
         let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
 
@@ -62,9 +44,8 @@ impl WVelocityConstraint {
         let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
         let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
 
-        let ccd_thickness1 = SimdReal::from(gather![|ii| bodies[handles1[ii]].ccd.ccd_thickness]);
-        let ccd_thickness2 = SimdReal::from(gather![|ii| bodies[handles2[ii]].ccd.ccd_thickness]);
-        let ccd_thickness = ccd_thickness1 + ccd_thickness2;
+        let poss1 = Isometry::from(gather![|ii| bodies[handles1[ii]].pos.position]);
+        let poss2 = Isometry::from(gather![|ii| bodies[handles2[ii]].pos.position]);
 
         let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
         let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);
@@ -84,8 +65,8 @@ impl WVelocityConstraint {
 
         let force_dir1 = -Vector::from(gather![|ii| manifolds[ii].data.normal]);
 
-        let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
+        let solver_vel1 = gather![|ii| ids1[ii].active_set_offset];
+        let solver_vel2 = gather![|ii| ids2[ii].active_set_offset];
 
         let num_active_contacts = manifolds[0].data.num_active_contacts();
 
@@ -99,22 +80,20 @@ impl WVelocityConstraint {
                 gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
-            let mut is_fast_contact = SimdBool::splat(false);
-            let mut constraint = WVelocityConstraint {
-                dir1: force_dir1,
-                #[cfg(feature = "dim3")]
-                tangent1: tangents1[0],
-                elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
-                im1,
-                im2,
-                cfm_factor,
-                limit: SimdReal::splat(0.0),
-                mj_lambda1,
-                mj_lambda2,
-                manifold_id,
-                manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
-                num_contacts: num_points as u8,
-            };
+            let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
+            let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
+
+            constraint.dir1 = force_dir1;
+            constraint.im1 = im1;
+            constraint.im2 = im2;
+            constraint.solver_vel1 = solver_vel1;
+            constraint.solver_vel2 = solver_vel2;
+            constraint.manifold_id = manifold_id;
+            constraint.num_contacts = num_points as u8;
+            #[cfg(feature = "dim3")]
+            {
+                constraint.tangent1 = tangents1[0];
+            }
 
             for k in 0..num_points {
                 let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
@@ -122,9 +101,10 @@ impl WVelocityConstraint {
                 let is_bouncy = SimdReal::from(gather![
                     |ii| manifold_points[ii][k].is_bouncy() as u32 as Real
                 ]);
-                let is_resting = SimdReal::splat(1.0) - is_bouncy;
-                let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
+
                 let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
+                let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
+
                 let tangent_velocity =
                     Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
                 let dp1 = point - world_com1;
@@ -137,6 +117,7 @@ impl WVelocityConstraint {
                 constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
 
                 // Normal part.
+                let normal_rhs_wo_bias;
                 {
                     let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
@@ -149,24 +130,15 @@ impl WVelocityConstraint {
                     );
 
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
-                    let mut rhs_wo_bias =
-                        (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
-                    rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting;
-                    let rhs_bias = (dist + allowed_lin_err)
-                        .simd_clamp(-max_penetration_correction, SimdReal::zero())
-                        * (erp_inv_dt/* * is_resting */);
+                    normal_rhs_wo_bias = is_bouncy * restitution * projected_velocity;
 
-                    let rhs = rhs_wo_bias + rhs_bias;
-                    is_fast_contact =
-                        is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
-
-                    constraint.elements[k].normal_part = VelocityConstraintNormalPart {
+                    constraint.elements[k].normal_part = TwoBodyConstraintNormalPart {
                         gcross1,
                         gcross2,
-                        rhs: rhs_wo_bias + rhs_bias,
-                        rhs_wo_bias,
+                        rhs: na::zero(),
+                        rhs_wo_bias: na::zero(),
                         impulse: SimdReal::splat(0.0),
+                        total_impulse: SimdReal::splat(0.0),
                         r: projected_mass,
                     };
                 }
@@ -181,11 +153,12 @@ impl WVelocityConstraint {
                     let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
                         + gcross1.gdot(gcross1)
                         + gcross2.gdot(gcross2);
-                    let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]);
+                    let rhs_wo_bias = tangent_velocity.dot(&tangents1[j]);
 
                     constraint.elements[k].tangent_part.gcross1[j] = gcross1;
                     constraint.elements[k].tangent_part.gcross2[j] = gcross2;
-                    constraint.elements[k].tangent_part.rhs[j] = rhs;
+                    constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
+                    constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
                     constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
                         utils::simd_inv(r)
                     } else {
@@ -201,40 +174,142 @@ impl WVelocityConstraint {
                             + constraint.elements[k].tangent_part.gcross2[0]
                                 .gdot(constraint.elements[k].tangent_part.gcross2[1]));
                 }
-            }
 
-            constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+                // Builder.
+                let infos = ContactPointInfos {
+                    local_p1: poss1.inverse_transform_point(&point),
+                    local_p2: poss2.inverse_transform_point(&point),
+                    tangent_vel: tangent_velocity,
+                    dist,
+                    normal_rhs_wo_bias,
+                };
 
-            if let Some(at) = insert_at {
-                out_constraints[at + l / MAX_MANIFOLD_POINTS] =
-                    AnyVelocityConstraint::Grouped(constraint);
-            } else {
-                out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
+                builder.infos[k] = infos;
             }
         }
     }
 
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        solved_dt: Real,
+        bodies: &[SolverBody],
+        _multibodies: &MultibodyJointSet,
+        constraint: &mut TwoBodyConstraintSimd,
+    ) {
+        let cfm_factor = SimdReal::splat(params.cfm_factor());
+        let dt = SimdReal::splat(params.dt);
+        let inv_dt = SimdReal::splat(params.inv_dt());
+        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
+        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
+        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
+
+        let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]];
+        let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
+
+        let ccd_thickness = SimdReal::from(gather![|ii| rb1[ii].ccd_thickness])
+            + SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
+
+        let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
+        let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
+
+        let all_infos = &self.infos[..constraint.num_contacts as usize];
+        let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
+
+        #[cfg(feature = "dim2")]
+        let tangents1 = constraint.dir1.orthonormal_basis();
+        #[cfg(feature = "dim3")]
+        let tangents1 = [
+            constraint.tangent1,
+            constraint.dir1.cross(&constraint.tangent1),
+        ];
+
+        let mut is_fast_contact = SimdBool::splat(false);
+        let solved_dt = SimdReal::splat(solved_dt);
+
+        for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
+            // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
+            let p1 = poss1 * info.local_p1 + info.tangent_vel * solved_dt;
+            let p2 = poss2 * info.local_p2;
+            let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
+
+            // Normal part.
+            {
+                let rhs_wo_bias =
+                    info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
+                let rhs_bias = (dist + allowed_lin_err)
+                    .simd_clamp(-max_penetration_correction, SimdReal::zero())
+                    * erp_inv_dt;
+                let new_rhs = rhs_wo_bias + rhs_bias;
+                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
+                is_fast_contact =
+                    is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
+
+                element.normal_part.rhs_wo_bias = rhs_wo_bias;
+                element.normal_part.rhs = new_rhs;
+                element.normal_part.total_impulse = total_impulse;
+                element.normal_part.impulse = na::zero();
+            }
+
+            // tangent parts.
+            {
+                element.tangent_part.total_impulse += element.tangent_part.impulse;
+                element.tangent_part.impulse = na::zero();
+
+                for j in 0..DIM - 1 {
+                    let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
+                    element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
+                }
+            }
+        }
+
+        constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct TwoBodyConstraintSimd {
+    pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
+    #[cfg(feature = "dim3")]
+    pub tangent1: Vector<SimdReal>, // One of the friction force directions.
+    pub elements: [TwoBodyConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
+    pub num_contacts: u8,
+    pub im1: Vector<SimdReal>,
+    pub im2: Vector<SimdReal>,
+    pub cfm_factor: SimdReal,
+    pub limit: SimdReal,
+    pub solver_vel1: [usize; SIMD_WIDTH],
+    pub solver_vel2: [usize; SIMD_WIDTH],
+    pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+    pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
+}
+
+impl TwoBodyConstraintSimd {
     pub fn solve(
         &mut self,
-        mj_lambdas: &mut [DeltaVel<Real>],
+        solver_vels: &mut [SolverVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
     ) {
-        let mut mj_lambda1 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
+        let mut solver_vel1 = SolverVel {
+            linear: Vector::from(gather![
+                |ii| solver_vels[self.solver_vel1[ii] as usize].linear
+            ]),
             angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
+                |ii| solver_vels[self.solver_vel1[ii] as usize].angular
             ]),
         };
 
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
+        let mut solver_vel2 = SolverVel {
+            linear: Vector::from(gather![
+                |ii| solver_vels[self.solver_vel2[ii] as usize].linear
+            ]),
             angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
+                |ii| solver_vels[self.solver_vel2[ii] as usize].angular
             ]),
         };
 
-        VelocityConstraintElement::solve_group(
+        TwoBodyConstraintElement::solve_group(
             self.cfm_factor,
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
@@ -243,19 +318,19 @@ impl WVelocityConstraint {
             &self.im1,
             &self.im2,
             self.limit,
-            &mut mj_lambda1,
-            &mut mj_lambda2,
+            &mut solver_vel1,
+            &mut solver_vel2,
             solve_normal,
             solve_friction,
         );
 
         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);
+            solver_vels[self.solver_vel1[ii] as usize].linear = solver_vel1.linear.extract(ii);
+            solver_vels[self.solver_vel1[ii] as usize].angular = solver_vel1.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);
+            solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii);
+            solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii);
         }
     }
 
@@ -289,6 +364,7 @@ impl WVelocityConstraint {
         self.cfm_factor = SimdReal::splat(1.0);
         for elt in &mut self.elements {
             elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
+            elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
         }
     }
 }

src/dynamics/solver/generic_velocity_ground_constraint.rs

@@ -1,240 +0,0 @@
-use crate::dynamics::solver::VelocityGroundConstraint;
-use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
-use crate::geometry::{ContactManifold, ContactManifoldIndex};
-use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
-use crate::utils::WCross;
-
-use super::{
-    AnyVelocityConstraint, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart,
-};
-#[cfg(feature = "dim2")]
-use crate::utils::WBasis;
-use na::DVector;
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct GenericVelocityGroundConstraint {
-    // We just build the generic constraint on top of the velocity constraint,
-    // adding some information we can use in the generic case.
-    pub velocity_constraint: VelocityGroundConstraint,
-    pub j_id: usize,
-    pub ndofs2: usize,
-}
-
-impl GenericVelocityGroundConstraint {
-    pub fn generate(
-        params: &IntegrationParameters,
-        manifold_id: ContactManifoldIndex,
-        manifold: &ContactManifold,
-        bodies: &RigidBodySet,
-        multibodies: &MultibodyJointSet,
-        out_constraints: &mut Vec<AnyVelocityConstraint>,
-        jacobians: &mut DVector<Real>,
-        jacobian_id: &mut usize,
-        insert_at: Option<usize>,
-    ) {
-        let cfm_factor = params.cfm_factor();
-        let inv_dt = params.inv_dt();
-        let erp_inv_dt = params.erp_inv_dt();
-
-        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 rb2 = &bodies[handle2.unwrap()];
-        let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
-
-        let (mb2, link_id2) = handle2
-            .and_then(|h| multibodies.rigid_body_link(h))
-            .map(|m| (&multibodies[m.multibody], m.id))
-            .unwrap();
-        let mj_lambda2 = 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 mut is_fast_contact = false;
-            let mut constraint = VelocityGroundConstraint {
-                dir1: force_dir1,
-                #[cfg(feature = "dim3")]
-                tangent1: tangents1[0],
-                elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
-                im2: mprops2.effective_inv_mass,
-                cfm_factor,
-                limit: 0.0,
-                mj_lambda2,
-                manifold_id,
-                manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
-                num_contacts: manifold_points.len() as u8,
-            };
-
-            for k in 0..manifold_points.len() {
-                let manifold_point = &manifold_points[k];
-                let dp1 = manifold_point.point - world_com1;
-                let dp2 = manifold_point.point - mprops2.world_com;
-
-                let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
-                let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
-
-                constraint.limit = manifold_point.friction;
-                constraint.manifold_contact_id[k] = manifold_point.contact_id;
-
-                // Normal part.
-                {
-                    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 is_resting = 1.0 - is_bouncy;
-
-                    let dvel = (vel1 - vel2).dot(&force_dir1);
-                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel;
-                    rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting;
-                    let rhs_bias =
-                        /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0);
-
-                    let rhs = rhs_wo_bias + rhs_bias;
-                    is_fast_contact =
-                        is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5);
-
-                    constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
-                        gcross2: na::zero(), // Unused for generic constraints.
-                        rhs,
-                        rhs_wo_bias,
-                        impulse: na::zero(),
-                        r,
-                    };
-                }
-
-                // Tangent parts.
-                {
-                    constraint.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 = (vel1 - vel2
-                            + flipped_multiplier * manifold_point.tangent_velocity)
-                            .dot(&tangents1[j]);
-
-                        constraint.elements[k].tangent_part.rhs[j] = rhs;
-
-                        // 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.elements[k].tangent_part.r[j] = r;
-                    }
-                }
-            }
-
-            constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
-
-            let constraint = GenericVelocityGroundConstraint {
-                velocity_constraint: constraint,
-                j_id: chunk_j_id,
-                ndofs2: mb2.ndofs(),
-            };
-
-            if let Some(at) = insert_at {
-                out_constraints[at + _l] =
-                    AnyVelocityConstraint::NongroupedGenericGround(constraint);
-            } else {
-                out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint));
-            }
-        }
-    }
-
-    pub fn solve(
-        &mut self,
-        jacobians: &DVector<Real>,
-        generic_mj_lambdas: &mut DVector<Real>,
-        solve_restitution: bool,
-        solve_friction: bool,
-    ) {
-        let mj_lambda2 = self.velocity_constraint.mj_lambda2 as usize;
-
-        let elements = &mut self.velocity_constraint.elements
-            [..self.velocity_constraint.num_contacts as usize];
-        VelocityGroundConstraintElement::generic_solve_group(
-            self.velocity_constraint.cfm_factor,
-            elements,
-            jacobians,
-            self.velocity_constraint.limit,
-            self.ndofs2,
-            self.j_id,
-            mj_lambda2,
-            generic_mj_lambdas,
-            solve_restitution,
-            solve_friction,
-        );
-    }
-
-    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
-        self.velocity_constraint.writeback_impulses(manifolds_all);
-    }
-
-    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
-        self.velocity_constraint.remove_cfm_and_bias_from_rhs();
-    }
-}

src/dynamics/solver/interaction_groups.rs

@@ -171,7 +171,7 @@ pub(crate) struct InteractionGroups {
     #[cfg(feature = "simd-is-enabled")]
     body_masks: Vec<u128>,
     #[cfg(feature = "simd-is-enabled")]
-    pub grouped_interactions: Vec<usize>,
+    pub simd_interactions: Vec<usize>,
     pub nongrouped_interactions: Vec<usize>,
 }
 
@@ -183,7 +183,7 @@ impl InteractionGroups {
             #[cfg(feature = "simd-is-enabled")]
             body_masks: Vec::new(),
             #[cfg(feature = "simd-is-enabled")]
-            grouped_interactions: Vec::new(),
+            simd_interactions: Vec::new(),
             nongrouped_interactions: Vec::new(),
         }
     }
@@ -194,7 +194,7 @@ impl InteractionGroups {
     //     {
     //         self.buckets.clear();
     //         self.body_masks.clear();
-    //         self.grouped_interactions.clear();
+    //         self.simd_interactions.clear();
     //     }
     //     self.nongrouped_interactions.clear();
     // }
@@ -300,7 +300,7 @@ impl InteractionGroups {
             if bucket.1 == SIMD_LAST_INDEX {
                 // We completed our group.
                 (bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
-                self.grouped_interactions.extend_from_slice(&bucket.0);
+                self.simd_interactions.extend_from_slice(&bucket.0);
                 bucket.1 = 0;
                 occupied_mask &= !target_mask_bit;
 
@@ -340,20 +340,20 @@ impl InteractionGroups {
         self.body_masks.iter_mut().for_each(|e| *e = 0);
 
         assert!(
-            self.grouped_interactions.len() % SIMD_WIDTH == 0,
+            self.simd_interactions.len() % SIMD_WIDTH == 0,
             "Invalid SIMD contact grouping."
         );
 
         //        println!(
         //            "Num grouped interactions: {}, nongrouped: {}",
-        //            self.grouped_interactions.len(),
+        //            self.simd_interactions.len(),
         //            self.nongrouped_interactions.len()
         //        );
     }
 
     pub fn clear_groups(&mut self) {
         #[cfg(feature = "simd-is-enabled")]
-        self.grouped_interactions.clear();
+        self.simd_interactions.clear();
         self.nongrouped_interactions.clear();
     }
 
@@ -466,7 +466,7 @@ impl InteractionGroups {
                 if bucket.1 == SIMD_LAST_INDEX {
                     // We completed our group.
                     (bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
-                    self.grouped_interactions.extend_from_slice(&bucket.0);
+                    self.simd_interactions.extend_from_slice(&bucket.0);
                     bucket.1 = 0;
                     occupied_mask = occupied_mask & (!target_mask_bit);
                 } else {
@@ -497,7 +497,7 @@ impl InteractionGroups {
         }
 
         assert!(
-            self.grouped_interactions.len() % SIMD_WIDTH == 0,
+            self.simd_interactions.len() % SIMD_WIDTH == 0,
             "Invalid SIMD contact grouping."
         );
     }

src/dynamics/solver/island_solver.rs

@@ -1,16 +1,15 @@
-use super::VelocitySolver;
+use super::{JointConstraintsSet, VelocitySolver};
 use crate::counters::Counters;
-use crate::dynamics::solver::{
-    AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints,
-};
+use crate::dynamics::solver::contact_constraint::ContactConstraintsSet;
 use crate::dynamics::IslandManager;
 use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::prelude::MultibodyJointSet;
+use parry::math::Real;
 
 pub struct IslandSolver {
-    contact_constraints: SolverConstraints<AnyVelocityConstraint>,
-    joint_constraints: SolverConstraints<AnyJointVelocityConstraint>,
+    contact_constraints: ContactConstraintsSet,
+    joint_constraints: JointConstraintsSet,
     velocity_solver: VelocitySolver,
 }
 
@@ -23,8 +22,8 @@ impl Default for IslandSolver {
 impl IslandSolver {
     pub fn new() -> Self {
         Self {
-            contact_constraints: SolverConstraints::new(),
-            joint_constraints: SolverConstraints::new(),
+            contact_constraints: ContactConstraintsSet::new(),
+            joint_constraints: JointConstraintsSet::new(),
             velocity_solver: VelocitySolver::new(),
         }
     }
@@ -33,57 +32,71 @@ impl IslandSolver {
         &mut self,
         island_id: usize,
         counters: &mut Counters,
-        params: &IntegrationParameters,
+        base_params: &IntegrationParameters,
         islands: &IslandManager,
         bodies: &mut RigidBodySet,
         manifolds: &mut [&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
         impulse_joints: &mut [JointGraphEdge],
         joint_indices: &[JointIndex],
-        multibody_joints: &mut MultibodyJointSet,
+        multibodies: &mut MultibodyJointSet,
     ) {
-        // Init the solver id for multibody_joints.
-        // We need that for building the constraints.
-        let mut solver_id = 0;
-        for (_, multibody) in multibody_joints.multibodies.iter_mut() {
-            multibody.solver_id = solver_id;
-            solver_id += multibody.ndofs();
-        }
+        counters.solver.velocity_resolution_time.resume();
+        let num_solver_iterations = base_params.num_solver_iterations.get()
+            + islands.active_island_additional_solver_iterations(island_id);
 
-        counters.solver.velocity_assembly_time.resume();
-        self.contact_constraints.init(
+        let mut params = *base_params;
+        params.dt /= num_solver_iterations as Real;
+        params.damping_ratio /= num_solver_iterations as Real;
+        // params.joint_damping_ratio /= num_solver_iterations as Real;
+
+        /*
+         *
+         * Bellow this point, the `params` is using the "small step" settings.
+         *
+         */
+        // INIT
+        self.velocity_solver
+            .init_solver_velocities_and_solver_bodies(
+                &params,
+                island_id,
+                islands,
+                bodies,
+                multibodies,
+            );
+        self.velocity_solver.init_constraints(
             island_id,
-            params,
             islands,
             bodies,
-            multibody_joints,
+            multibodies,
             manifolds,
             manifold_indices,
-        );
-        self.joint_constraints.init(
-            island_id,
-            params,
-            islands,
-            bodies,
-            multibody_joints,
             impulse_joints,
             joint_indices,
+            &mut self.contact_constraints,
+            &mut self.joint_constraints,
         );
-        counters.solver.velocity_assembly_time.pause();
 
-        counters.solver.velocity_resolution_time.resume();
-        self.velocity_solver.solve(
-            island_id,
-            params,
-            islands,
+        // SOLVE
+        self.velocity_solver.solve_constraints(
+            &params,
+            num_solver_iterations,
             bodies,
-            multibody_joints,
-            manifolds,
-            impulse_joints,
-            &mut self.contact_constraints.velocity_constraints,
-            &self.contact_constraints.generic_jacobians,
-            &mut self.joint_constraints.velocity_constraints,
-            &self.joint_constraints.generic_jacobians,
+            multibodies,
+            &mut self.contact_constraints,
+            &mut self.joint_constraints,
+        );
+
+        // WRITEBACK
+        self.joint_constraints.writeback_impulses(impulse_joints);
+        self.contact_constraints.writeback_impulses(manifolds);
+        self.velocity_solver.writeback_bodies(
+            base_params,
+            num_solver_iterations,
+            islands,
+            island_id,
+            bodies,
+            multibodies,
         );
         counters.solver.velocity_resolution_time.pause();
     }

src/dynamics/solver/joint_constraint/any_joint_constraint.rs

@@ -0,0 +1,97 @@
+use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{
+    JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
+};
+use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
+    JointOneBodyConstraint, JointTwoBodyConstraint,
+};
+use crate::dynamics::solver::{AnyConstraintMut, ConstraintTypes};
+use crate::dynamics::JointGraphEdge;
+use crate::math::Real;
+use na::DVector;
+
+use crate::dynamics::solver::joint_constraint::joint_generic_constraint_builder::{
+    JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder,
+};
+use crate::dynamics::solver::solver_vel::SolverVel;
+#[cfg(feature = "simd-is-enabled")]
+use crate::{
+    dynamics::solver::joint_constraint::joint_constraint_builder::{
+        JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
+    },
+    math::{SimdReal, SIMD_WIDTH},
+};
+
+use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
+    JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder,
+};
+
+pub struct JointConstraintTypes;
+
+impl<'a> AnyConstraintMut<'a, JointConstraintTypes> {
+    pub fn remove_bias(&mut self) {
+        match self {
+            Self::OneBody(c) => c.remove_bias_from_rhs(),
+            Self::TwoBodies(c) => c.remove_bias_from_rhs(),
+            Self::GenericOneBody(c) => c.remove_bias_from_rhs(),
+            Self::GenericTwoBodies(c) => c.remove_bias_from_rhs(),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.remove_bias_from_rhs(),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.remove_bias_from_rhs(),
+        }
+    }
+
+    pub fn solve(
+        &mut self,
+        generic_jacobians: &DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        match self {
+            Self::OneBody(c) => c.solve(solver_vels),
+            Self::TwoBodies(c) => c.solve(solver_vels),
+            Self::GenericOneBody(c) => c.solve(generic_jacobians, solver_vels, generic_solver_vels),
+            Self::GenericTwoBodies(c) => {
+                c.solve(generic_jacobians, solver_vels, generic_solver_vels)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.solve(solver_vels),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.solve(solver_vels),
+        }
+    }
+
+    pub fn writeback_impulses(&mut self, joints_all: &mut [JointGraphEdge]) {
+        match self {
+            Self::OneBody(c) => c.writeback_impulses(joints_all),
+            Self::TwoBodies(c) => c.writeback_impulses(joints_all),
+            Self::GenericOneBody(c) => c.writeback_impulses(joints_all),
+            Self::GenericTwoBodies(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.writeback_impulses(joints_all),
+        }
+    }
+}
+
+impl ConstraintTypes for JointConstraintTypes {
+    type OneBody = JointOneBodyConstraint<Real, 1>;
+    type TwoBodies = JointTwoBodyConstraint<Real, 1>;
+    type GenericOneBody = JointGenericOneBodyConstraint;
+    type GenericTwoBodies = JointGenericTwoBodyConstraint;
+
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdOneBody = JointOneBodyConstraint<SimdReal, SIMD_WIDTH>;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdTwoBodies = JointTwoBodyConstraint<SimdReal, SIMD_WIDTH>;
+
+    type BuilderOneBody = JointOneBodyConstraintBuilder;
+    type BuilderTwoBodies = JointTwoBodyConstraintBuilder;
+    type GenericBuilderOneBody = JointGenericOneBodyConstraintBuilder;
+    type GenericBuilderTwoBodies = JointGenericTwoBodyConstraintBuilder;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdBuilderOneBody = JointOneBodyConstraintBuilderSimd;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdBuilderTwoBodies = JointTwoBodyConstraintBuilderSimd;
+}

src/dynamics/solver/joint_constraint/joint_constraint.rs

@@ -1,540 +0,0 @@
-use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{
-    JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
-};
-use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
-    JointVelocityConstraint, JointVelocityGroundConstraint, SolverBody,
-};
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
-};
-use crate::math::{Real, SPATIAL_DIM};
-use crate::prelude::MultibodyJointSet;
-use na::DVector;
-
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::{Isometry, SimdReal, SIMD_WIDTH};
-
-#[cfg(feature = "parallel")]
-use crate::dynamics::JointAxesMask;
-
-pub enum AnyJointVelocityConstraint {
-    JointConstraint(JointVelocityConstraint<Real, 1>),
-    JointGroundConstraint(JointVelocityGroundConstraint<Real, 1>),
-    JointGenericConstraint(JointGenericVelocityConstraint),
-    JointGenericGroundConstraint(JointGenericVelocityGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    JointConstraintSimd(JointVelocityConstraint<SimdReal, SIMD_WIDTH>),
-    #[cfg(feature = "simd-is-enabled")]
-    JointGroundConstraintSimd(JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH>),
-    Empty,
-}
-
-impl AnyJointVelocityConstraint {
-    #[cfg(feature = "parallel")]
-    pub fn num_active_constraints_and_jacobian_lines(joint: &ImpulseJoint) -> (usize, usize) {
-        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;
-        (num_constraints, num_constraints)
-    }
-
-    pub fn from_joint(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        joint: &ImpulseJoint,
-        bodies: &RigidBodySet,
-        multibodies: &MultibodyJointSet,
-        j_id: &mut usize,
-        jacobians: &mut DVector<Real>,
-        out: &mut Vec<Self>,
-        insert_at: Option<usize>,
-    ) {
-        let local_frame1 = joint.data.local_frame1;
-        let local_frame2 = joint.data.local_frame2;
-        let rb1 = &bodies[joint.body1];
-        let rb2 = &bodies[joint.body2];
-        let frame1 = rb1.pos.position * local_frame1;
-        let frame2 = rb2.pos.position * local_frame2;
-
-        let body1 = SolverBody {
-            linvel: rb1.vels.linvel,
-            angvel: rb1.vels.angvel,
-            im: rb1.mprops.effective_inv_mass,
-            sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
-            world_com: rb1.mprops.world_com,
-            mj_lambda: [rb1.ids.active_set_offset],
-        };
-        let body2 = SolverBody {
-            linvel: rb2.vels.linvel,
-            angvel: rb2.vels.angvel,
-            im: rb2.mprops.effective_inv_mass,
-            sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
-            world_com: rb2.mprops.world_com,
-            mj_lambda: [rb2.ids.active_set_offset],
-        };
-
-        let mb1 = multibodies
-            .rigid_body_link(joint.body1)
-            .map(|link| (&multibodies[link.multibody], link.id));
-        let mb2 = multibodies
-            .rigid_body_link(joint.body2)
-            .map(|link| (&multibodies[link.multibody], link.id));
-
-        if mb1.is_some() || mb2.is_some() {
-            let multibodies_ndof = mb1.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM)
-                + mb2.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM);
-
-            if multibodies_ndof == 0 {
-                // Both multibodies are fixed, don’t generate any constraint.
-                return;
-            }
-
-            // For each solver contact we generate up to SPATIAL_DIM constraints, and each
-            // constraints appends the multibodies jacobian and weighted jacobians.
-            // Also note that for impulse_joints, the rigid-bodies will also add their jacobians
-            // to the generic DVector.
-            // TODO: is this count correct when we take both motors and limits into account?
-            let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
-
-            if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
-                jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
-            }
-
-            // TODO: find a way to avoid the temporary buffer.
-            let mut out_tmp = [JointGenericVelocityConstraint::invalid(); 12];
-            let out_tmp_len = JointGenericVelocityConstraint::lock_axes(
-                params,
-                joint_id,
-                &body1,
-                &body2,
-                mb1,
-                mb2,
-                &frame1,
-                &frame2,
-                &joint.data,
-                jacobians,
-                j_id,
-                &mut out_tmp,
-            );
-
-            if let Some(at) = insert_at {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out[at + i] = AnyJointVelocityConstraint::JointGenericConstraint(c);
-                }
-            } else {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
-                }
-            }
-        } else {
-            // TODO: find a way to avoid the temporary buffer.
-            let mut out_tmp = [JointVelocityConstraint::invalid(); 12];
-            let out_tmp_len = JointVelocityConstraint::<Real, 1>::lock_axes(
-                params,
-                joint_id,
-                &body1,
-                &body2,
-                &frame1,
-                &frame2,
-                &joint.data,
-                &mut out_tmp,
-            );
-
-            if let Some(at) = insert_at {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out[at + i] = AnyJointVelocityConstraint::JointConstraint(c);
-                }
-            } else {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out.push(AnyJointVelocityConstraint::JointConstraint(c));
-                }
-            }
-        }
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
-        bodies: &RigidBodySet,
-        out: &mut Vec<Self>,
-        insert_at: Option<usize>,
-    ) {
-        use crate::dynamics::{
-            RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-        };
-
-        let rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ) = (
-            gather![|ii| &bodies[impulse_joints[ii].body1].pos],
-            gather![|ii| &bodies[impulse_joints[ii].body1].vels],
-            gather![|ii| &bodies[impulse_joints[ii].body1].mprops],
-            gather![|ii| &bodies[impulse_joints[ii].body1].ids],
-        );
-        let rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ) = (
-            gather![|ii| &bodies[impulse_joints[ii].body2].pos],
-            gather![|ii| &bodies[impulse_joints[ii].body2].vels],
-            gather![|ii| &bodies[impulse_joints[ii].body2].mprops],
-            gather![|ii| &bodies[impulse_joints[ii].body2].ids],
-        );
-
-        let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1;
-        let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2;
-        let pos1: Isometry<SimdReal> = gather![|ii| rb_pos1[ii].position].into();
-        let pos2: Isometry<SimdReal> = gather![|ii| rb_pos2[ii].position].into();
-
-        let local_frame1: Isometry<SimdReal> =
-            gather![|ii| impulse_joints[ii].data.local_frame1].into();
-        let local_frame2: Isometry<SimdReal> =
-            gather![|ii| impulse_joints[ii].data.local_frame2].into();
-
-        let frame1 = pos1 * local_frame1;
-        let frame2 = pos2 * local_frame2;
-
-        let body1: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
-            linvel: gather![|ii| rb_vel1[ii].linvel].into(),
-            angvel: gather![|ii| rb_vel1[ii].angvel].into(),
-            im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(),
-            sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(),
-            world_com: gather![|ii| rb_mprops1[ii].world_com].into(),
-            mj_lambda: gather![|ii| rb_ids1[ii].active_set_offset],
-        };
-        let body2: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
-            linvel: gather![|ii| rb_vel2[ii].linvel].into(),
-            angvel: gather![|ii| rb_vel2[ii].angvel].into(),
-            im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(),
-            sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(),
-            world_com: gather![|ii| rb_mprops2[ii].world_com].into(),
-            mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset],
-        };
-
-        // TODO: find a way to avoid the temporary buffer.
-        let mut out_tmp = [JointVelocityConstraint::invalid(); 12];
-        let out_tmp_len = JointVelocityConstraint::<SimdReal, SIMD_WIDTH>::lock_axes(
-            params,
-            joint_id,
-            &body1,
-            &body2,
-            &frame1,
-            &frame2,
-            impulse_joints[0].data.locked_axes.bits(),
-            &mut out_tmp,
-        );
-
-        if let Some(at) = insert_at {
-            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                out[at + i] = AnyJointVelocityConstraint::JointConstraintSimd(c);
-            }
-        } else {
-            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
-            }
-        }
-    }
-
-    pub fn from_joint_ground(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        joint: &ImpulseJoint,
-        bodies: &RigidBodySet,
-        multibodies: &MultibodyJointSet,
-        j_id: &mut usize,
-        jacobians: &mut DVector<Real>,
-        out: &mut Vec<Self>,
-        insert_at: Option<usize>,
-    ) {
-        let mut handle1 = joint.body1;
-        let mut handle2 = joint.body2;
-        let flipped = !bodies[handle2].is_dynamic();
-
-        let (local_frame1, local_frame2) = if flipped {
-            std::mem::swap(&mut handle1, &mut handle2);
-            (joint.data.local_frame2, joint.data.local_frame1)
-        } else {
-            (joint.data.local_frame1, joint.data.local_frame2)
-        };
-
-        let rb1 = &bodies[handle1];
-        let rb2 = &bodies[handle2];
-
-        let frame1 = rb1.pos.position * local_frame1;
-        let frame2 = rb2.pos.position * local_frame2;
-
-        let body1 = SolverBody {
-            linvel: rb1.vels.linvel,
-            angvel: rb1.vels.angvel,
-            im: rb1.mprops.effective_inv_mass,
-            sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
-            world_com: rb1.mprops.world_com,
-            mj_lambda: [crate::INVALID_USIZE],
-        };
-        let body2 = SolverBody {
-            linvel: rb2.vels.linvel,
-            angvel: rb2.vels.angvel,
-            im: rb2.mprops.effective_inv_mass,
-            sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
-            world_com: rb2.mprops.world_com,
-            mj_lambda: [rb2.ids.active_set_offset],
-        };
-
-        if let Some(mb2) = multibodies
-            .rigid_body_link(handle2)
-            .map(|link| (&multibodies[link.multibody], link.id))
-        {
-            let multibodies_ndof = mb2.0.ndofs();
-
-            if multibodies_ndof == 0 {
-                // The multibody is fixed, don’t generate any constraint.
-                return;
-            }
-
-            // For each solver contact we generate up to SPATIAL_DIM constraints, and each
-            // constraints appends the multibodies jacobian and weighted jacobians.
-            // Also note that for impulse_joints, the rigid-bodies will also add their jacobians
-            // to the generic DVector.
-            // TODO: is this count correct when we take both motors and limits into account?
-            let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
-
-            if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
-                jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
-            }
-
-            // TODO: find a way to avoid the temporary buffer.
-            let mut out_tmp = [JointGenericVelocityGroundConstraint::invalid(); 12];
-            let out_tmp_len = JointGenericVelocityGroundConstraint::lock_axes(
-                params,
-                joint_id,
-                &body1,
-                &body2,
-                mb2,
-                &frame1,
-                &frame2,
-                &joint.data,
-                jacobians,
-                j_id,
-                &mut out_tmp,
-            );
-
-            if let Some(at) = insert_at {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out[at + i] = AnyJointVelocityConstraint::JointGenericGroundConstraint(c);
-                }
-            } else {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
-                }
-            }
-        } else {
-            // TODO: find a way to avoid the temporary buffer.
-            let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12];
-            let out_tmp_len = JointVelocityGroundConstraint::<Real, 1>::lock_axes(
-                params,
-                joint_id,
-                &body1,
-                &body2,
-                &frame1,
-                &frame2,
-                &joint.data,
-                &mut out_tmp,
-            );
-
-            if let Some(at) = insert_at {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out[at + i] = AnyJointVelocityConstraint::JointGroundConstraint(c);
-                }
-            } else {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
-                }
-            }
-        }
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint_ground(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
-        bodies: &RigidBodySet,
-        out: &mut Vec<Self>,
-        insert_at: Option<usize>,
-    ) {
-        use crate::dynamics::{
-            RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
-        };
-
-        let mut handles1 = gather![|ii| impulse_joints[ii].body1];
-        let mut handles2 = gather![|ii| impulse_joints[ii].body2];
-        let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].body_type];
-        let mut flipped = [false; SIMD_WIDTH];
-
-        for ii in 0..SIMD_WIDTH {
-            if !status2[ii].is_dynamic() {
-                std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
-                flipped[ii] = true;
-            }
-        }
-
-        let local_frame1: Isometry<SimdReal> = gather![|ii| if flipped[ii] {
-            impulse_joints[ii].data.local_frame2
-        } else {
-            impulse_joints[ii].data.local_frame1
-        }]
-        .into();
-        let local_frame2: Isometry<SimdReal> = gather![|ii| if flipped[ii] {
-            impulse_joints[ii].data.local_frame1
-        } else {
-            impulse_joints[ii].data.local_frame2
-        }]
-        .into();
-
-        let rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-        ) = (
-            gather![|ii| &bodies[handles1[ii]].pos],
-            gather![|ii| &bodies[handles1[ii]].vels],
-            gather![|ii| &bodies[handles1[ii]].mprops],
-        );
-        let rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ) = (
-            gather![|ii| &bodies[handles2[ii]].pos],
-            gather![|ii| &bodies[handles2[ii]].vels],
-            gather![|ii| &bodies[handles2[ii]].mprops],
-            gather![|ii| &bodies[handles2[ii]].ids],
-        );
-
-        let (rb_pos1, rb_vel1, rb_mprops1) = rbs1;
-        let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2;
-        let pos1: Isometry<SimdReal> = gather![|ii| rb_pos1[ii].position].into();
-        let pos2: Isometry<SimdReal> = gather![|ii| rb_pos2[ii].position].into();
-
-        let frame1 = pos1 * local_frame1;
-        let frame2 = pos2 * local_frame2;
-
-        let body1: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
-            linvel: gather![|ii| rb_vel1[ii].linvel].into(),
-            angvel: gather![|ii| rb_vel1[ii].angvel].into(),
-            im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(),
-            sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(),
-            world_com: gather![|ii| rb_mprops1[ii].world_com].into(),
-            mj_lambda: [crate::INVALID_USIZE; SIMD_WIDTH],
-        };
-        let body2: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
-            linvel: gather![|ii| rb_vel2[ii].linvel].into(),
-            angvel: gather![|ii| rb_vel2[ii].angvel].into(),
-            im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(),
-            sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(),
-            world_com: gather![|ii| rb_mprops2[ii].world_com].into(),
-            mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset],
-        };
-
-        // TODO: find a way to avoid the temporary buffer.
-        let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12];
-        let out_tmp_len = JointVelocityGroundConstraint::<SimdReal, SIMD_WIDTH>::lock_axes(
-            params,
-            joint_id,
-            &body1,
-            &body2,
-            &frame1,
-            &frame2,
-            impulse_joints[0].data.locked_axes.bits(),
-            &mut out_tmp,
-        );
-
-        if let Some(at) = insert_at {
-            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                out[at + i] = AnyJointVelocityConstraint::JointGroundConstraintSimd(c);
-            }
-        } else {
-            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
-            }
-        }
-    }
-
-    pub fn remove_bias_from_rhs(&mut self) {
-        match self {
-            AnyJointVelocityConstraint::JointConstraint(c) => c.remove_bias_from_rhs(),
-            AnyJointVelocityConstraint::JointGroundConstraint(c) => c.remove_bias_from_rhs(),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointConstraintSimd(c) => c.remove_bias_from_rhs(),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.remove_bias_from_rhs(),
-            AnyJointVelocityConstraint::JointGenericConstraint(c) => c.remove_bias_from_rhs(),
-            AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => c.remove_bias_from_rhs(),
-            AnyJointVelocityConstraint::Empty => unreachable!(),
-        }
-    }
-
-    pub fn solve(
-        &mut self,
-        jacobians: &DVector<Real>,
-        mj_lambdas: &mut [DeltaVel<Real>],
-        generic_mj_lambdas: &mut DVector<Real>,
-    ) {
-        match self {
-            AnyJointVelocityConstraint::JointConstraint(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::JointGroundConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointConstraintSimd(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::JointGenericConstraint(c) => {
-                c.solve(jacobians, mj_lambdas, generic_mj_lambdas)
-            }
-            AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => {
-                c.solve(jacobians, mj_lambdas, generic_mj_lambdas)
-            }
-            AnyJointVelocityConstraint::Empty => unreachable!(),
-        }
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        match self {
-            AnyJointVelocityConstraint::JointConstraint(c) => c.writeback_impulses(joints_all),
-            AnyJointVelocityConstraint::JointGroundConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointConstraintSimd(c) => c.writeback_impulses(joints_all),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            AnyJointVelocityConstraint::JointGenericConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            AnyJointVelocityConstraint::Empty => unreachable!(),
-        }
-    }
-}

src/dynamics/solver/joint_constraint/joint_constraints_set.rs

@@ -0,0 +1,446 @@
+use crate::dynamics::solver::categorization::categorize_joints;
+use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::solver_vel::SolverVel;
+use crate::dynamics::solver::{
+    reset_buffer, JointConstraintTypes, JointGenericOneBodyConstraint,
+    JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraint,
+    JointGenericTwoBodyConstraintBuilder, JointGenericVelocityOneBodyExternalConstraintBuilder,
+    JointGenericVelocityOneBodyInternalConstraintBuilder, SolverConstraintsSet,
+};
+use crate::dynamics::{
+    IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
+    RigidBodySet,
+};
+use na::DVector;
+use parry::math::Real;
+
+use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
+    JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder,
+};
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
+        JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
+    },
+    crate::math::SIMD_WIDTH,
+};
+
+pub type JointConstraintsSet = SolverConstraintsSet<JointConstraintTypes>;
+
+impl JointConstraintsSet {
+    pub fn init(
+        &mut self,
+        island_id: usize,
+        islands: &IslandManager,
+        bodies: &RigidBodySet,
+        multibody_joints: &MultibodyJointSet,
+        impulse_joints: &[JointGraphEdge],
+        joint_constraint_indices: &[JointIndex],
+    ) {
+        // Generate constraints for impulse_joints.
+        self.two_body_interactions.clear();
+        self.one_body_interactions.clear();
+        self.generic_two_body_interactions.clear();
+        self.generic_one_body_interactions.clear();
+
+        categorize_joints(
+            bodies,
+            multibody_joints,
+            impulse_joints,
+            joint_constraint_indices,
+            &mut self.one_body_interactions,
+            &mut self.two_body_interactions,
+            &mut self.generic_one_body_interactions,
+            &mut self.generic_two_body_interactions,
+        );
+
+        self.clear_constraints();
+        self.clear_builders();
+
+        self.interaction_groups.clear_groups();
+        self.interaction_groups.group_joints(
+            island_id,
+            islands,
+            bodies,
+            impulse_joints,
+            &self.two_body_interactions,
+        );
+
+        self.one_body_interaction_groups.clear_groups();
+        self.one_body_interaction_groups.group_joints(
+            island_id,
+            islands,
+            bodies,
+            impulse_joints,
+            &self.one_body_interactions,
+        );
+        // NOTE: uncomment this do disable SIMD joint 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);
+
+        let mut j_id = 0;
+        self.compute_joint_constraints(bodies, impulse_joints);
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.simd_compute_joint_constraints(bodies, impulse_joints);
+        }
+        self.compute_generic_joint_constraints(bodies, multibody_joints, impulse_joints, &mut j_id);
+
+        self.compute_joint_one_body_constraints(bodies, impulse_joints);
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.simd_compute_joint_one_body_constraints(bodies, impulse_joints);
+        }
+        self.compute_generic_one_body_joint_constraints(
+            island_id,
+            islands,
+            bodies,
+            multibody_joints,
+            impulse_joints,
+            &mut j_id,
+        );
+    }
+
+    fn compute_joint_one_body_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        let total_num_builders = self
+            .one_body_interaction_groups
+            .nongrouped_interactions
+            .len();
+
+        unsafe {
+            reset_buffer(
+                &mut self.velocity_one_body_constraints_builder,
+                total_num_builders,
+            );
+        }
+
+        let mut num_constraints = 0;
+        for (joint_i, builder) in self
+            .one_body_interaction_groups
+            .nongrouped_interactions
+            .iter()
+            .zip(self.velocity_one_body_constraints_builder.iter_mut())
+        {
+            let joint = &joints_all[*joint_i].weight;
+            JointOneBodyConstraintBuilder::generate(
+                joint,
+                bodies,
+                *joint_i,
+                builder,
+                &mut num_constraints,
+            );
+        }
+
+        unsafe {
+            reset_buffer(&mut self.velocity_one_body_constraints, num_constraints);
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn simd_compute_joint_one_body_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        let total_num_builders =
+            self.one_body_interaction_groups.simd_interactions.len() / SIMD_WIDTH;
+
+        unsafe {
+            reset_buffer(
+                &mut self.simd_velocity_one_body_constraints_builder,
+                total_num_builders,
+            );
+        }
+
+        let mut num_constraints = 0;
+        for (joints_i, builder) in self
+            .one_body_interaction_groups
+            .simd_interactions
+            .chunks_exact(SIMD_WIDTH)
+            .zip(self.simd_velocity_one_body_constraints_builder.iter_mut())
+        {
+            let joints_id = gather![|ii| joints_i[ii]];
+            let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
+            JointOneBodyConstraintBuilderSimd::generate(
+                impulse_joints,
+                bodies,
+                joints_id,
+                builder,
+                &mut num_constraints,
+            );
+        }
+
+        unsafe {
+            reset_buffer(
+                &mut self.simd_velocity_one_body_constraints,
+                num_constraints,
+            );
+        }
+    }
+
+    fn compute_joint_constraints(&mut self, bodies: &RigidBodySet, joints_all: &[JointGraphEdge]) {
+        let total_num_builders = self.interaction_groups.nongrouped_interactions.len();
+
+        unsafe {
+            reset_buffer(&mut self.velocity_constraints_builder, total_num_builders);
+        }
+
+        let mut num_constraints = 0;
+        for (joint_i, builder) in self
+            .interaction_groups
+            .nongrouped_interactions
+            .iter()
+            .zip(self.velocity_constraints_builder.iter_mut())
+        {
+            let joint = &joints_all[*joint_i].weight;
+            JointTwoBodyConstraintBuilder::generate(
+                joint,
+                bodies,
+                *joint_i,
+                builder,
+                &mut num_constraints,
+            );
+        }
+
+        unsafe {
+            reset_buffer(&mut self.velocity_constraints, num_constraints);
+        }
+    }
+
+    fn compute_generic_joint_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        multibodies: &MultibodyJointSet,
+        joints_all: &[JointGraphEdge],
+        j_id: &mut usize,
+    ) {
+        let total_num_builders = self.generic_two_body_interactions.len();
+        self.generic_velocity_constraints_builder.resize(
+            total_num_builders,
+            JointGenericTwoBodyConstraintBuilder::invalid(),
+        );
+
+        let mut num_constraints = 0;
+        for (joint_i, builder) in self
+            .generic_two_body_interactions
+            .iter()
+            .zip(self.generic_velocity_constraints_builder.iter_mut())
+        {
+            let joint = &joints_all[*joint_i].weight;
+            JointGenericTwoBodyConstraintBuilder::generate(
+                *joint_i,
+                joint,
+                bodies,
+                multibodies,
+                builder,
+                j_id,
+                &mut self.generic_jacobians,
+                &mut num_constraints,
+            );
+        }
+
+        self.generic_velocity_constraints
+            .resize(num_constraints, JointGenericTwoBodyConstraint::invalid());
+    }
+
+    fn compute_generic_one_body_joint_constraints(
+        &mut self,
+        island_id: usize,
+        islands: &IslandManager,
+        bodies: &RigidBodySet,
+        multibodies: &MultibodyJointSet,
+        joints_all: &[JointGraphEdge],
+        j_id: &mut usize,
+    ) {
+        let mut total_num_builders = self.generic_one_body_interactions.len();
+
+        for handle in islands.active_island(island_id) {
+            if let Some(link_id) = multibodies.rigid_body_link(*handle) {
+                if JointGenericVelocityOneBodyInternalConstraintBuilder::num_constraints(
+                    multibodies,
+                    link_id,
+                ) > 0
+                {
+                    total_num_builders += 1;
+                }
+            }
+        }
+
+        self.generic_velocity_one_body_constraints_builder.resize(
+            total_num_builders,
+            JointGenericOneBodyConstraintBuilder::invalid(),
+        );
+
+        let mut num_constraints = 0;
+        for (joint_i, builder) in self.generic_one_body_interactions.iter().zip(
+            self.generic_velocity_one_body_constraints_builder
+                .iter_mut(),
+        ) {
+            let joint = &joints_all[*joint_i].weight;
+            JointGenericVelocityOneBodyExternalConstraintBuilder::generate(
+                *joint_i,
+                joint,
+                bodies,
+                multibodies,
+                builder,
+                j_id,
+                &mut self.generic_jacobians,
+                &mut num_constraints,
+            );
+        }
+
+        let mut curr_builder = self.generic_one_body_interactions.len();
+        for handle in islands.active_island(island_id) {
+            if curr_builder >= self.generic_velocity_one_body_constraints_builder.len() {
+                break; // No more builder need to be generated.
+            }
+
+            if let Some(link_id) = multibodies.rigid_body_link(*handle) {
+                let prev_num_constraints = num_constraints;
+                JointGenericVelocityOneBodyInternalConstraintBuilder::generate(
+                    multibodies,
+                    link_id,
+                    &mut self.generic_velocity_one_body_constraints_builder[curr_builder],
+                    j_id,
+                    &mut self.generic_jacobians,
+                    &mut num_constraints,
+                );
+                if num_constraints != prev_num_constraints {
+                    curr_builder += 1;
+                }
+            }
+        }
+
+        self.generic_velocity_one_body_constraints
+            .resize(num_constraints, JointGenericOneBodyConstraint::invalid());
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn simd_compute_joint_constraints(
+        &mut self,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        let total_num_builders = self.interaction_groups.simd_interactions.len() / SIMD_WIDTH;
+
+        unsafe {
+            reset_buffer(
+                &mut self.simd_velocity_constraints_builder,
+                total_num_builders,
+            );
+        }
+
+        let mut num_constraints = 0;
+        for (joints_i, builder) in self
+            .interaction_groups
+            .simd_interactions
+            .chunks_exact(SIMD_WIDTH)
+            .zip(self.simd_velocity_constraints_builder.iter_mut())
+        {
+            let joints_id = gather![|ii| joints_i[ii]];
+            let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
+            JointTwoBodyConstraintBuilderSimd::generate(
+                impulse_joints,
+                bodies,
+                joints_id,
+                builder,
+                &mut num_constraints,
+            );
+        }
+
+        unsafe {
+            reset_buffer(&mut self.simd_velocity_constraints, num_constraints);
+        }
+    }
+
+    pub fn solve(
+        &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(jac, solver_vels, generic_solver_vels);
+        }
+    }
+
+    pub fn solve_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(jac, solver_vels, generic_solver_vels);
+        }
+    }
+
+    pub fn writeback_impulses(&mut self, joints_all: &mut [JointGraphEdge]) {
+        let (_, constraints) = self.iter_constraints_mut();
+        for mut c in constraints {
+            c.writeback_impulses(joints_all);
+        }
+    }
+
+    pub fn update(
+        &mut self,
+        params: &IntegrationParameters,
+        multibodies: &MultibodyJointSet,
+        solver_bodies: &[SolverBody],
+    ) {
+        for builder in &mut self.generic_velocity_constraints_builder {
+            builder.update(
+                &params,
+                multibodies,
+                solver_bodies,
+                &mut self.generic_jacobians,
+                &mut self.generic_velocity_constraints,
+            );
+        }
+
+        for builder in &mut self.generic_velocity_one_body_constraints_builder {
+            builder.update(
+                &params,
+                multibodies,
+                solver_bodies,
+                &mut self.generic_jacobians,
+                &mut self.generic_velocity_one_body_constraints,
+            );
+        }
+
+        for builder in &mut self.velocity_constraints_builder {
+            builder.update(&params, solver_bodies, &mut self.velocity_constraints);
+        }
+
+        #[cfg(feature = "simd-is-enabled")]
+        for builder in &mut self.simd_velocity_constraints_builder {
+            builder.update(&params, solver_bodies, &mut self.simd_velocity_constraints);
+        }
+
+        for builder in &mut self.velocity_one_body_constraints_builder {
+            builder.update(
+                &params,
+                solver_bodies,
+                &mut self.velocity_one_body_constraints,
+            );
+        }
+
+        #[cfg(feature = "simd-is-enabled")]
+        for builder in &mut self.simd_velocity_one_body_constraints_builder {
+            builder.update(
+                &params,
+                solver_bodies,
+                &mut self.simd_velocity_one_body_constraints,
+            );
+        }
+    }
+}

src/dynamics/solver/joint_constraint/joint_generic_constraint.rs

@@ -1,17 +1,19 @@
-use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
-use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
-use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
+    JointFixedSolverBody, WritebackId,
+};
+use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper};
+use crate::dynamics::solver::SolverVel;
 use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody};
 use crate::math::{Isometry, Real, DIM};
 use crate::prelude::SPATIAL_DIM;
 use na::{DVector, DVectorView, DVectorViewMut};
 
 #[derive(Debug, Copy, Clone)]
-pub struct JointGenericVelocityConstraint {
+pub struct JointGenericTwoBodyConstraint {
     pub is_rigid_body1: bool,
     pub is_rigid_body2: bool,
-    pub mj_lambda1: usize,
-    pub mj_lambda2: usize,
+    pub solver_vel1: usize,
+    pub solver_vel2: usize,
 
     pub ndofs1: usize,
     pub j_id1: usize,
@@ -31,31 +33,31 @@ pub struct JointGenericVelocityConstraint {
     pub writeback_id: WritebackId,
 }
 
-impl Default for JointGenericVelocityConstraint {
+impl Default for JointGenericTwoBodyConstraint {
     fn default() -> Self {
-        JointGenericVelocityConstraint::invalid()
+        JointGenericTwoBodyConstraint::invalid()
     }
 }
 
-impl JointGenericVelocityConstraint {
+impl JointGenericTwoBodyConstraint {
     pub fn invalid() -> Self {
         Self {
             is_rigid_body1: false,
             is_rigid_body2: false,
-            mj_lambda1: 0,
-            mj_lambda2: 0,
-            ndofs1: 0,
-            j_id1: 0,
-            ndofs2: 0,
-            j_id2: 0,
-            joint_id: 0,
+            solver_vel1: usize::MAX,
+            solver_vel2: usize::MAX,
+            ndofs1: usize::MAX,
+            j_id1: usize::MAX,
+            ndofs2: usize::MAX,
+            j_id2: usize::MAX,
+            joint_id: usize::MAX,
             impulse: 0.0,
             impulse_bounds: [-Real::MAX, Real::MAX],
-            inv_lhs: 0.0,
-            rhs: 0.0,
-            rhs_wo_bias: 0.0,
-            cfm_coeff: 0.0,
-            cfm_gain: 0.0,
+            inv_lhs: Real::MAX,
+            rhs: Real::MAX,
+            rhs_wo_bias: Real::MAX,
+            cfm_coeff: Real::MAX,
+            cfm_gain: Real::MAX,
             writeback_id: WritebackId::Dof(0),
         }
     }
@@ -63,8 +65,8 @@ impl JointGenericVelocityConstraint {
     pub fn lock_axes(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
         frame1: &Isometry<Real>,
@@ -79,7 +81,7 @@ impl JointGenericVelocityConstraint {
         let motor_axes = joint.motor_axes.bits();
         let limit_axes = joint.limit_axes.bits();
 
-        let builder = JointVelocityConstraintBuilder::new(
+        let builder = JointTwoBodyConstraintHelper::new(
             frame1,
             frame2,
             &body1.world_com,
@@ -91,7 +93,6 @@ impl JointGenericVelocityConstraint {
         for i in DIM..SPATIAL_DIM {
             if motor_axes & (1 << i) != 0 {
                 out[len] = builder.motor_angular_generic(
-                    params,
                     jacobians,
                     j_id,
                     joint_id,
@@ -109,7 +110,6 @@ impl JointGenericVelocityConstraint {
         for i in 0..DIM {
             if motor_axes & (1 << i) != 0 {
                 out[len] = builder.motor_linear_generic(
-                    params,
                     jacobians,
                     j_id,
                     joint_id,
@@ -125,10 +125,7 @@ impl JointGenericVelocityConstraint {
                 len += 1;
             }
         }
-        JointVelocityConstraintBuilder::finalize_generic_constraints(
-            jacobians,
-            &mut out[start..len],
-        );
+        JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]);
 
         let start = len;
         for i in DIM..SPATIAL_DIM {
@@ -203,10 +200,7 @@ impl JointGenericVelocityConstraint {
             }
         }
 
-        JointVelocityConstraintBuilder::finalize_generic_constraints(
-            jacobians,
-            &mut out[start..len],
-        );
+        JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]);
         len
     }
 
@@ -218,69 +212,69 @@ impl JointGenericVelocityConstraint {
         self.j_id2 + self.ndofs2
     }
 
-    fn mj_lambda1<'a>(
+    fn solver_vel1<'a>(
         &self,
-        mj_lambdas: &'a [DeltaVel<Real>],
-        generic_mj_lambdas: &'a DVector<Real>,
+        solver_vels: &'a [SolverVel<Real>],
+        generic_solver_vels: &'a DVector<Real>,
     ) -> DVectorView<'a, Real> {
         if self.is_rigid_body1 {
-            mj_lambdas[self.mj_lambda1].as_vector_slice()
+            solver_vels[self.solver_vel1].as_vector_slice()
         } else {
-            generic_mj_lambdas.rows(self.mj_lambda1, self.ndofs1)
+            generic_solver_vels.rows(self.solver_vel1, self.ndofs1)
         }
     }
 
-    fn mj_lambda1_mut<'a>(
+    fn solver_vel1_mut<'a>(
         &self,
-        mj_lambdas: &'a mut [DeltaVel<Real>],
-        generic_mj_lambdas: &'a mut DVector<Real>,
+        solver_vels: &'a mut [SolverVel<Real>],
+        generic_solver_vels: &'a mut DVector<Real>,
     ) -> DVectorViewMut<'a, Real> {
         if self.is_rigid_body1 {
-            mj_lambdas[self.mj_lambda1].as_vector_slice_mut()
+            solver_vels[self.solver_vel1].as_vector_slice_mut()
         } else {
-            generic_mj_lambdas.rows_mut(self.mj_lambda1, self.ndofs1)
+            generic_solver_vels.rows_mut(self.solver_vel1, self.ndofs1)
         }
     }
 
-    fn mj_lambda2<'a>(
+    fn solver_vel2<'a>(
         &self,
-        mj_lambdas: &'a [DeltaVel<Real>],
-        generic_mj_lambdas: &'a DVector<Real>,
+        solver_vels: &'a [SolverVel<Real>],
+        generic_solver_vels: &'a DVector<Real>,
     ) -> DVectorView<'a, Real> {
         if self.is_rigid_body2 {
-            mj_lambdas[self.mj_lambda2].as_vector_slice()
+            solver_vels[self.solver_vel2].as_vector_slice()
         } else {
-            generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2)
+            generic_solver_vels.rows(self.solver_vel2, self.ndofs2)
         }
     }
 
-    fn mj_lambda2_mut<'a>(
+    fn solver_vel2_mut<'a>(
         &self,
-        mj_lambdas: &'a mut [DeltaVel<Real>],
-        generic_mj_lambdas: &'a mut DVector<Real>,
+        solver_vels: &'a mut [SolverVel<Real>],
+        generic_solver_vels: &'a mut DVector<Real>,
     ) -> DVectorViewMut<'a, Real> {
         if self.is_rigid_body2 {
-            mj_lambdas[self.mj_lambda2].as_vector_slice_mut()
+            solver_vels[self.solver_vel2].as_vector_slice_mut()
         } else {
-            generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2)
+            generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2)
         }
     }
 
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,
-        mj_lambdas: &mut [DeltaVel<Real>],
-        generic_mj_lambdas: &mut DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
     ) {
         let jacobians = jacobians.as_slice();
 
-        let mj_lambda1 = self.mj_lambda1(mj_lambdas, generic_mj_lambdas);
+        let solver_vel1 = self.solver_vel1(solver_vels, generic_solver_vels);
         let j1 = DVectorView::from_slice(&jacobians[self.j_id1..], self.ndofs1);
-        let vel1 = j1.dot(&mj_lambda1);
+        let vel1 = j1.dot(&solver_vel1);
 
-        let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas);
+        let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels);
         let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2);
-        let vel2 = j2.dot(&mj_lambda2);
+        let vel2 = j2.dot(&solver_vel2);
 
         let dvel = self.rhs + (vel2 - vel1);
         let total_impulse = na::clamp(
@@ -291,13 +285,13 @@ impl JointGenericVelocityConstraint {
         let delta_impulse = total_impulse - self.impulse;
         self.impulse = total_impulse;
 
-        let mut mj_lambda1 = self.mj_lambda1_mut(mj_lambdas, generic_mj_lambdas);
+        let mut solver_vel1 = self.solver_vel1_mut(solver_vels, generic_solver_vels);
         let wj1 = DVectorView::from_slice(&jacobians[self.wj_id1()..], self.ndofs1);
-        mj_lambda1.axpy(delta_impulse, &wj1, 1.0);
+        solver_vel1.axpy(delta_impulse, &wj1, 1.0);
 
-        let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas);
+        let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels);
         let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
-        mj_lambda2.axpy(-delta_impulse, &wj2, 1.0);
+        solver_vel2.axpy(-delta_impulse, &wj2, 1.0);
     }
 
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
@@ -315,8 +309,8 @@ impl JointGenericVelocityConstraint {
 }
 
 #[derive(Debug, Copy, Clone)]
-pub struct JointGenericVelocityGroundConstraint {
-    pub mj_lambda2: usize,
+pub struct JointGenericOneBodyConstraint {
+    pub solver_vel2: usize,
     pub ndofs2: usize,
     pub j_id2: usize,
 
@@ -333,16 +327,16 @@ pub struct JointGenericVelocityGroundConstraint {
     pub writeback_id: WritebackId,
 }
 
-impl Default for JointGenericVelocityGroundConstraint {
+impl Default for JointGenericOneBodyConstraint {
     fn default() -> Self {
-        JointGenericVelocityGroundConstraint::invalid()
+        JointGenericOneBodyConstraint::invalid()
     }
 }
 
-impl JointGenericVelocityGroundConstraint {
+impl JointGenericOneBodyConstraint {
     pub fn invalid() -> Self {
         Self {
-            mj_lambda2: crate::INVALID_USIZE,
+            solver_vel2: crate::INVALID_USIZE,
             ndofs2: 0,
             j_id2: crate::INVALID_USIZE,
             joint_id: 0,
@@ -360,8 +354,8 @@ impl JointGenericVelocityGroundConstraint {
     pub fn lock_axes(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointFixedSolverBody<Real>,
+        body2: &JointSolverBody<Real, 1>,
         mb2: (&Multibody, usize),
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
@@ -375,7 +369,7 @@ impl JointGenericVelocityGroundConstraint {
         let motor_axes = joint.motor_axes.bits();
         let limit_axes = joint.limit_axes.bits();
 
-        let builder = JointVelocityConstraintBuilder::new(
+        let builder = JointTwoBodyConstraintHelper::new(
             frame1,
             frame2,
             &body1.world_com,
@@ -386,13 +380,11 @@ impl JointGenericVelocityGroundConstraint {
         let start = len;
         for i in DIM..SPATIAL_DIM {
             if motor_axes & (1 << i) != 0 {
-                out[len] = builder.motor_angular_generic_ground(
-                    params,
+                out[len] = builder.motor_angular_generic_one_body(
                     jacobians,
                     j_id,
                     joint_id,
                     body1,
-                    body2,
                     mb2,
                     i - DIM,
                     &joint.motors[i].motor_params(params.dt),
@@ -404,13 +396,11 @@ impl JointGenericVelocityGroundConstraint {
 
         for i in 0..DIM {
             if motor_axes & (1 << i) != 0 {
-                out[len] = builder.motor_linear_generic_ground(
-                    params,
+                out[len] = builder.motor_linear_generic_one_body(
                     jacobians,
                     j_id,
                     joint_id,
                     body1,
-                    body2,
                     mb2,
                     // locked_ang_axes,
                     i,
@@ -421,7 +411,7 @@ impl JointGenericVelocityGroundConstraint {
             }
         }
 
-        JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
+        JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body(
             jacobians,
             &mut out[start..len],
         );
@@ -429,7 +419,7 @@ impl JointGenericVelocityGroundConstraint {
         let start = len;
         for i in DIM..SPATIAL_DIM {
             if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_angular_generic_ground(
+                out[len] = builder.lock_angular_generic_one_body(
                     params,
                     jacobians,
                     j_id,
@@ -444,7 +434,7 @@ impl JointGenericVelocityGroundConstraint {
         }
         for i in 0..DIM {
             if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_linear_generic_ground(
+                out[len] = builder.lock_linear_generic_one_body(
                     params,
                     jacobians,
                     j_id,
@@ -460,7 +450,7 @@ impl JointGenericVelocityGroundConstraint {
 
         for i in DIM..SPATIAL_DIM {
             if limit_axes & (1 << i) != 0 {
-                out[len] = builder.limit_angular_generic_ground(
+                out[len] = builder.limit_angular_generic_one_body(
                     params,
                     jacobians,
                     j_id,
@@ -476,7 +466,7 @@ impl JointGenericVelocityGroundConstraint {
         }
         for i in 0..DIM {
             if limit_axes & (1 << i) != 0 {
-                out[len] = builder.limit_linear_generic_ground(
+                out[len] = builder.limit_linear_generic_one_body(
                     params,
                     jacobians,
                     j_id,
@@ -491,7 +481,7 @@ impl JointGenericVelocityGroundConstraint {
             }
         }
 
-        JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
+        JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body(
             jacobians,
             &mut out[start..len],
         );
@@ -502,33 +492,33 @@ impl JointGenericVelocityGroundConstraint {
         self.j_id2 + self.ndofs2
     }
 
-    fn mj_lambda2<'a>(
+    fn solver_vel2<'a>(
         &self,
-        _mj_lambdas: &'a [DeltaVel<Real>],
-        generic_mj_lambdas: &'a DVector<Real>,
+        _solver_vels: &'a [SolverVel<Real>],
+        generic_solver_vels: &'a DVector<Real>,
     ) -> DVectorView<'a, Real> {
-        generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2)
+        generic_solver_vels.rows(self.solver_vel2, self.ndofs2)
     }
 
-    fn mj_lambda2_mut<'a>(
+    fn solver_vel2_mut<'a>(
         &self,
-        _mj_lambdas: &'a mut [DeltaVel<Real>],
-        generic_mj_lambdas: &'a mut DVector<Real>,
+        _solver_vels: &'a mut [SolverVel<Real>],
+        generic_solver_vels: &'a mut DVector<Real>,
     ) -> DVectorViewMut<'a, Real> {
-        generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2)
+        generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2)
     }
 
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,
-        mj_lambdas: &mut [DeltaVel<Real>],
-        generic_mj_lambdas: &mut DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
     ) {
         let jacobians = jacobians.as_slice();
 
-        let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas);
+        let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels);
         let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2);
-        let vel2 = j2.dot(&mj_lambda2);
+        let vel2 = j2.dot(&solver_vel2);
 
         let dvel = self.rhs + vel2;
         let total_impulse = na::clamp(
@@ -539,9 +529,9 @@ impl JointGenericVelocityGroundConstraint {
         let delta_impulse = total_impulse - self.impulse;
         self.impulse = total_impulse;
 
-        let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas);
+        let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels);
         let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
-        mj_lambda2.axpy(-delta_impulse, &wj2, 1.0);
+        solver_vel2.axpy(-delta_impulse, &wj2, 1.0);
     }
 
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {

src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs

@@ -1,29 +1,459 @@
-use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{
-    JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
+use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{
+    JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
 };
-use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
-use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
+use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
+    JointFixedSolverBody, WritebackId,
+};
+use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper};
 use crate::dynamics::solver::MotorParameters;
-use crate::dynamics::{IntegrationParameters, JointIndex, Multibody};
+use crate::dynamics::{
+    GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex, Multibody, MultibodyJointSet,
+    MultibodyLinkId, RigidBodySet,
+};
 use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM};
 use crate::utils;
 use crate::utils::IndexMut2;
-use crate::utils::WDot;
+use crate::utils::SimdDot;
 use na::{DVector, SVector};
 
+use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::ConstraintsCounts;
 #[cfg(feature = "dim3")]
-use crate::utils::WAngularInertia;
+use crate::utils::SimdAngularInertia;
 #[cfg(feature = "dim2")]
 use na::Vector1;
+use parry::math::Isometry;
 
-impl SolverBody<Real, 1> {
+#[derive(Copy, Clone)]
+enum LinkOrBody {
+    Link(MultibodyLinkId),
+    Body(usize),
+}
+
+#[derive(Copy, Clone)]
+pub struct JointGenericTwoBodyConstraintBuilder {
+    link1: LinkOrBody,
+    link2: LinkOrBody,
+    joint_id: JointIndex,
+    joint: GenericJoint,
+    j_id: usize,
+    // These are solver body for both joints, except that
+    // the world_com is actually in local-space.
+    local_body1: JointSolverBody<Real, 1>,
+    local_body2: JointSolverBody<Real, 1>,
+    multibodies_ndof: usize,
+    constraint_id: usize,
+}
+
+impl JointGenericTwoBodyConstraintBuilder {
+    pub fn invalid() -> Self {
+        Self {
+            link1: LinkOrBody::Body(usize::MAX),
+            link2: LinkOrBody::Body(usize::MAX),
+            joint_id: JointIndex::MAX,
+            joint: GenericJoint::default(),
+            j_id: usize::MAX,
+            local_body1: JointSolverBody::invalid(),
+            local_body2: JointSolverBody::invalid(),
+            multibodies_ndof: usize::MAX,
+            constraint_id: usize::MAX,
+        }
+    }
+
+    pub fn generate(
+        joint_id: JointIndex,
+        joint: &ImpulseJoint,
+        bodies: &RigidBodySet,
+        multibodies: &MultibodyJointSet,
+        out_builder: &mut Self,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+        out_constraint_id: &mut usize,
+    ) {
+        let starting_j_id = *j_id;
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
+
+        let local_body1 = JointSolverBody {
+            im: rb1.mprops.effective_inv_mass,
+            sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
+            world_com: rb1.mprops.local_mprops.local_com,
+            solver_vel: [rb1.ids.active_set_offset],
+        };
+        let local_body2 = JointSolverBody {
+            im: rb2.mprops.effective_inv_mass,
+            sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
+            world_com: rb2.mprops.local_mprops.local_com,
+            solver_vel: [rb2.ids.active_set_offset],
+        };
+
+        let mut multibodies_ndof = 0;
+        let link1 = match multibodies.rigid_body_link(joint.body1) {
+            Some(link) => {
+                multibodies_ndof += multibodies[link.multibody].ndofs();
+                LinkOrBody::Link(*link)
+            }
+            None => {
+                multibodies_ndof += SPATIAL_DIM;
+                LinkOrBody::Body(rb2.ids.active_set_offset)
+            }
+        };
+        let link2 = match multibodies.rigid_body_link(joint.body2) {
+            Some(link) => {
+                multibodies_ndof += multibodies[link.multibody].ndofs();
+                LinkOrBody::Link(*link)
+            }
+            None => {
+                multibodies_ndof += SPATIAL_DIM;
+                LinkOrBody::Body(rb2.ids.active_set_offset)
+            }
+        };
+
+        if multibodies_ndof == 0 {
+            // Both multibodies are fixed, don’t generate any constraint.
+            out_builder.multibodies_ndof = multibodies_ndof;
+            return;
+        }
+
+        // For each solver contact we generate up to SPATIAL_DIM constraints, and each
+        // constraints appends the multibodies jacobian and weighted jacobians.
+        // Also note that for impulse_joints, the rigid-bodies will also add their jacobians
+        // to the generic DVector.
+        // TODO: is this count correct when we take both motors and limits into account?
+        let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
+
+        // TODO: use a more precise increment.
+        *j_id += multibodies_ndof * 2 * SPATIAL_DIM;
+
+        if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
+            jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
+        }
+
+        *out_builder = Self {
+            link1,
+            link2,
+            joint_id,
+            joint: joint.data,
+            j_id: starting_j_id,
+            local_body1,
+            local_body2,
+            multibodies_ndof,
+            constraint_id: *out_constraint_id,
+        };
+
+        *out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints;
+    }
+
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        multibodies: &MultibodyJointSet,
+        bodies: &[SolverBody],
+        jacobians: &mut DVector<Real>,
+        out: &mut [JointGenericTwoBodyConstraint],
+    ) {
+        if self.multibodies_ndof == 0 {
+            // The joint is between two static bodies, no constraint needed.
+            return;
+        }
+
+        // NOTE: right now, the "update", is basically reconstructing all the
+        //       constraints. Could we make this more incremental?
+        let pos1;
+        let pos2;
+        let mb1;
+        let mb2;
+
+        match self.link1 {
+            LinkOrBody::Link(link) => {
+                let mb = &multibodies[link.multibody];
+                pos1 = &mb.link(link.id).unwrap().local_to_world;
+                mb1 = Some((mb, link.id));
+            }
+            LinkOrBody::Body(body1) => {
+                pos1 = &bodies[body1].position;
+                mb1 = None;
+            }
+        };
+        match self.link2 {
+            LinkOrBody::Link(link) => {
+                let mb = &multibodies[link.multibody];
+                pos2 = &mb.link(link.id).unwrap().local_to_world;
+                mb2 = Some((mb, link.id));
+            }
+            LinkOrBody::Body(body2) => {
+                pos2 = &bodies[body2].position;
+                mb2 = None;
+            }
+        };
+
+        let frame1 = pos1 * self.joint.local_frame1;
+        let frame2 = pos2 * self.joint.local_frame2;
+
+        let joint_body1 = JointSolverBody {
+            world_com: pos1 * self.local_body1.world_com, // the world_com was stored in local-space.
+            ..self.local_body1
+        };
+        let joint_body2 = JointSolverBody {
+            world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space.
+            ..self.local_body2
+        };
+
+        let mut j_id = self.j_id;
+
+        JointGenericTwoBodyConstraint::lock_axes(
+            params,
+            self.joint_id,
+            &joint_body1,
+            &joint_body2,
+            mb1,
+            mb2,
+            &frame1,
+            &frame2,
+            &self.joint,
+            jacobians,
+            &mut j_id,
+            &mut out[self.constraint_id..],
+        );
+    }
+}
+
+#[derive(Copy, Clone)]
+pub enum JointGenericOneBodyConstraintBuilder {
+    Internal(JointGenericVelocityOneBodyInternalConstraintBuilder),
+    External(JointGenericVelocityOneBodyExternalConstraintBuilder),
+    Empty,
+}
+
+#[derive(Copy, Clone)]
+pub struct JointGenericVelocityOneBodyInternalConstraintBuilder {
+    link: MultibodyLinkId,
+    j_id: usize,
+    constraint_id: usize,
+}
+
+impl JointGenericOneBodyConstraintBuilder {
+    pub fn invalid() -> Self {
+        Self::Empty
+    }
+
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        multibodies: &MultibodyJointSet,
+        bodies: &[SolverBody],
+        jacobians: &mut DVector<Real>,
+        out: &mut [JointGenericOneBodyConstraint],
+    ) {
+        match self {
+            Self::Empty => {}
+            Self::Internal(builder) => builder.update(params, multibodies, jacobians, out),
+            Self::External(builder) => builder.update(params, multibodies, bodies, jacobians, out),
+        }
+    }
+}
+
+impl JointGenericVelocityOneBodyInternalConstraintBuilder {
+    pub fn num_constraints(multibodies: &MultibodyJointSet, link_id: &MultibodyLinkId) -> usize {
+        let multibody = &multibodies[link_id.multibody];
+        let link = multibody.link(link_id.id).unwrap();
+        link.joint().num_velocity_constraints()
+    }
+
+    pub fn generate(
+        multibodies: &MultibodyJointSet,
+        link_id: &MultibodyLinkId,
+        out_builder: &mut JointGenericOneBodyConstraintBuilder,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+        out_constraint_id: &mut usize,
+    ) {
+        let multibody = &multibodies[link_id.multibody];
+        let link = multibody.link(link_id.id).unwrap();
+        let num_constraints = link.joint().num_velocity_constraints();
+
+        if num_constraints == 0 {
+            return;
+        }
+
+        *out_builder = JointGenericOneBodyConstraintBuilder::Internal(Self {
+            link: *link_id,
+            j_id: *j_id,
+            constraint_id: *out_constraint_id,
+        });
+
+        *j_id += num_constraints * multibody.ndofs() * 2;
+        if jacobians.nrows() < *j_id {
+            jacobians.resize_vertically_mut(*j_id, 0.0);
+        }
+
+        *out_constraint_id += num_constraints;
+    }
+
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        multibodies: &MultibodyJointSet,
+        jacobians: &mut DVector<Real>,
+        out: &mut [JointGenericOneBodyConstraint],
+    ) {
+        let mb = &multibodies[self.link.multibody];
+        let link = mb.link(self.link.id).unwrap();
+        link.joint().velocity_constraints(
+            params,
+            mb,
+            link,
+            self.j_id,
+            jacobians,
+            &mut out[self.constraint_id..],
+        );
+    }
+}
+
+#[derive(Copy, Clone)]
+pub struct JointGenericVelocityOneBodyExternalConstraintBuilder {
+    body1: JointFixedSolverBody<Real>,
+    frame1: Isometry<Real>,
+    link2: MultibodyLinkId,
+    joint_id: JointIndex,
+    joint: GenericJoint,
+    j_id: usize,
+    flipped: bool,
+    constraint_id: usize,
+    // These are solver body for both joints, except that
+    // the world_com is actually in local-space.
+    local_body2: JointSolverBody<Real, 1>,
+}
+
+impl JointGenericVelocityOneBodyExternalConstraintBuilder {
+    pub fn generate(
+        joint_id: JointIndex,
+        joint: &ImpulseJoint,
+        bodies: &RigidBodySet,
+        multibodies: &MultibodyJointSet,
+        out_builder: &mut JointGenericOneBodyConstraintBuilder,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+        out_constraint_id: &mut usize,
+    ) {
+        let mut handle1 = joint.body1;
+        let mut handle2 = joint.body2;
+        let flipped = !bodies[handle2].is_dynamic();
+
+        let local_frame1 = if flipped {
+            std::mem::swap(&mut handle1, &mut handle2);
+            joint.data.local_frame2
+        } else {
+            joint.data.local_frame1
+        };
+
+        let rb1 = &bodies[handle1];
+        let rb2 = &bodies[handle2];
+
+        let frame1 = rb1.pos.position * local_frame1;
+
+        let starting_j_id = *j_id;
+
+        let body1 = JointFixedSolverBody {
+            linvel: rb1.vels.linvel,
+            angvel: rb1.vels.angvel,
+            world_com: rb1.mprops.world_com,
+        };
+        let local_body2 = JointSolverBody {
+            im: rb2.mprops.effective_inv_mass,
+            sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
+            world_com: rb2.mprops.local_mprops.local_com,
+            solver_vel: [rb2.ids.active_set_offset],
+        };
+
+        let link2 = *multibodies.rigid_body_link(handle2).unwrap();
+        let mb2 = &multibodies[link2.multibody];
+        let multibodies_ndof = mb2.ndofs();
+
+        if multibodies_ndof == 0 {
+            // The multibody is fixed, don’t generate any constraint.
+            *out_builder = JointGenericOneBodyConstraintBuilder::Empty;
+            return;
+        }
+
+        // For each solver contact we generate up to SPATIAL_DIM constraints, and each
+        // constraints appends the multibodies jacobian and weighted jacobians.
+        // Also note that for impulse_joints, the rigid-bodies will also add their jacobians
+        // to the generic DVector.
+        // TODO: is this count correct when we take both motors and limits into account?
+        let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
+        // TODO: use a more precise increment.
+        *j_id += multibodies_ndof * 2 * SPATIAL_DIM;
+
+        if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
+            jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
+        }
+
+        *out_builder = JointGenericOneBodyConstraintBuilder::External(Self {
+            body1,
+            link2,
+            joint_id,
+            joint: joint.data,
+            j_id: starting_j_id,
+            frame1,
+            local_body2,
+            flipped,
+            constraint_id: *out_constraint_id,
+        });
+
+        *out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints;
+    }
+
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        multibodies: &MultibodyJointSet,
+        _bodies: &[SolverBody],
+        jacobians: &mut DVector<Real>,
+        out: &mut [JointGenericOneBodyConstraint],
+    ) {
+        // NOTE: right now, the "update", is basically reconstructing all the
+        //       constraints. Could we make this more incremental?
+        let mb2 = &multibodies[self.link2.multibody];
+        let pos2 = &mb2.link(self.link2.id).unwrap().local_to_world;
+        let frame2 = pos2
+            * if self.flipped {
+                self.joint.local_frame1
+            } else {
+                self.joint.local_frame2
+            };
+
+        let joint_body2 = JointSolverBody {
+            world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space.
+            ..self.local_body2
+        };
+
+        let mut j_id = self.j_id;
+
+        JointGenericOneBodyConstraint::lock_axes(
+            params,
+            self.joint_id,
+            &self.body1,
+            &joint_body2,
+            (mb2, self.link2.id),
+            &self.frame1,
+            &frame2,
+            &self.joint,
+            jacobians,
+            &mut j_id,
+            &mut out[self.constraint_id..],
+        );
+    }
+}
+
+impl JointSolverBody<Real, 1> {
     pub fn fill_jacobians(
         &self,
         unit_force: Vector<Real>,
         unit_torque: SVector<Real, ANG_DIM>,
         j_id: &mut usize,
         jacobians: &mut DVector<Real>,
-    ) -> Real {
+    ) {
         let wj_id = *j_id + SPATIAL_DIM;
         jacobians
             .fixed_rows_mut::<DIM>(*j_id)
@@ -54,26 +484,24 @@ impl SolverBody<Real, 1> {
         }
 
         *j_id += SPATIAL_DIM * 2;
-        unit_force.dot(&self.linvel) + unit_torque.gdot(self.angvel)
     }
 }
 
-impl JointVelocityConstraintBuilder<Real> {
+impl JointTwoBodyConstraintHelper<Real> {
     pub fn lock_jacobians_generic(
         &self,
-        _params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
         writeback_id: WritebackId,
         lin_jac: Vector<Real>,
         ang_jac1: SVector<Real, ANG_DIM>,
         ang_jac2: SVector<Real, ANG_DIM>,
-    ) -> JointGenericVelocityConstraint {
+    ) -> JointGenericTwoBodyConstraint {
         let is_rigid_body1 = mb1.is_none();
         let is_rigid_body2 = mb2.is_none();
 
@@ -81,19 +509,17 @@ impl JointVelocityConstraintBuilder<Real> {
         let ndofs2 = mb2.map(|(m, _)| m.ndofs()).unwrap_or(SPATIAL_DIM);
 
         let j_id1 = *j_id;
-        let vel1 = if let Some((mb1, link_id1)) = mb1 {
-            mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians)
-                .1
+        if let Some((mb1, link_id1)) = mb1 {
+            mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians);
         } else {
-            body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians)
+            body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians);
         };
 
         let j_id2 = *j_id;
-        let vel2 = if let Some((mb2, link_id2)) = mb2 {
-            mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
-                .1
+        if let Some((mb2, link_id2)) = mb2 {
+            mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians);
         } else {
-            body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians)
+            body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians);
         };
 
         if is_rigid_body1 {
@@ -116,16 +542,16 @@ impl JointVelocityConstraintBuilder<Real> {
             j.copy_from(&wj);
         }
 
-        let rhs_wo_bias = vel2 - vel1;
+        let rhs_wo_bias = 0.0;
 
-        let mj_lambda1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.mj_lambda[0]);
-        let mj_lambda2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.mj_lambda[0]);
+        let solver_vel1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.solver_vel[0]);
+        let solver_vel2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.solver_vel[0]);
 
-        JointGenericVelocityConstraint {
+        JointGenericTwoBodyConstraint {
             is_rigid_body1,
             is_rigid_body2,
-            mj_lambda1,
-            mj_lambda2,
+            solver_vel1,
+            solver_vel2,
             ndofs1,
             j_id1,
             ndofs2,
@@ -148,19 +574,18 @@ impl JointVelocityConstraintBuilder<Real> {
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
         locked_axis: usize,
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityConstraint {
+    ) -> JointGenericTwoBodyConstraint {
         let lin_jac = self.basis.column(locked_axis).into_owned();
         let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
         let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
 
         let mut c = self.lock_jacobians_generic(
-            params,
             jacobians,
             j_id,
             joint_id,
@@ -186,20 +611,19 @@ impl JointVelocityConstraintBuilder<Real> {
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
         limited_axis: usize,
         limits: [Real; 2],
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityConstraint {
+    ) -> JointGenericTwoBodyConstraint {
         let lin_jac = self.basis.column(limited_axis).into_owned();
         let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned();
         let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned();
 
         let mut constraint = self.lock_jacobians_generic(
-            params,
             jacobians,
             j_id,
             joint_id,
@@ -230,18 +654,17 @@ impl JointVelocityConstraintBuilder<Real> {
 
     pub fn motor_linear_generic(
         &self,
-        params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
         motor_axis: usize,
         motor_params: &MotorParameters<Real>,
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityConstraint {
+    ) -> JointGenericTwoBodyConstraint {
         let lin_jac = self.basis.column(motor_axis).into_owned();
         let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
         let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
@@ -255,7 +678,6 @@ impl JointVelocityConstraintBuilder<Real> {
         // }
 
         let mut constraint = self.lock_jacobians_generic(
-            params,
             jacobians,
             j_id,
             joint_id,
@@ -275,9 +697,7 @@ impl JointVelocityConstraintBuilder<Real> {
             rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
         }
 
-        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
-            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-        rhs_wo_bias += dvel - motor_params.target_vel;
+        rhs_wo_bias += -motor_params.target_vel;
 
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint.rhs = rhs_wo_bias;
@@ -293,20 +713,19 @@ impl JointVelocityConstraintBuilder<Real> {
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
         _locked_axis: usize,
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityConstraint {
+    ) -> JointGenericTwoBodyConstraint {
         #[cfg(feature = "dim2")]
         let ang_jac = Vector1::new(1.0);
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
 
         let mut constraint = self.lock_jacobians_generic(
-            params,
             jacobians,
             j_id,
             joint_id,
@@ -335,21 +754,20 @@ impl JointVelocityConstraintBuilder<Real> {
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
         _limited_axis: usize,
         limits: [Real; 2],
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityConstraint {
+    ) -> JointGenericTwoBodyConstraint {
         #[cfg(feature = "dim2")]
         let ang_jac = Vector1::new(1.0);
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
 
         let mut constraint = self.lock_jacobians_generic(
-            params,
             jacobians,
             j_id,
             joint_id,
@@ -386,25 +804,23 @@ impl JointVelocityConstraintBuilder<Real> {
 
     pub fn motor_angular_generic(
         &self,
-        params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
         _motor_axis: usize,
         motor_params: &MotorParameters<Real>,
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityConstraint {
+    ) -> JointGenericTwoBodyConstraint {
         #[cfg(feature = "dim2")]
         let ang_jac = na::Vector1::new(1.0);
         #[cfg(feature = "dim3")]
         let ang_jac = self.basis.column(_motor_axis).into_owned();
 
         let mut constraint = self.lock_jacobians_generic(
-            params,
             jacobians,
             j_id,
             joint_id,
@@ -429,8 +845,7 @@ impl JointVelocityConstraintBuilder<Real> {
                 * motor_params.erp_inv_dt;
         }
 
-        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        rhs_wo_bias += dvel - motor_params.target_vel;
+        rhs_wo_bias += -motor_params.target_vel;
 
         constraint.rhs_wo_bias = rhs_wo_bias;
         constraint.rhs = rhs_wo_bias;
@@ -442,7 +857,7 @@ impl JointVelocityConstraintBuilder<Real> {
 
     pub fn finalize_generic_constraints(
         jacobians: &mut DVector<Real>,
-        constraints: &mut [JointGenericVelocityConstraint],
+        constraints: &mut [JointGenericTwoBodyConstraint],
     ) {
         // TODO: orthogonalization doesn’t seem to give good results for multibodies?
         const ORTHOGONALIZE: bool = false;
@@ -506,34 +921,30 @@ impl JointVelocityConstraintBuilder<Real> {
     }
 }
 
-impl JointVelocityConstraintBuilder<Real> {
-    pub fn lock_jacobians_generic_ground(
+impl JointTwoBodyConstraintHelper<Real> {
+    pub fn lock_jacobians_generic_one_body(
         &self,
-        _params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
+        body1: &JointFixedSolverBody<Real>,
         (mb2, link_id2): (&Multibody, usize),
         writeback_id: WritebackId,
         lin_jac: Vector<Real>,
         ang_jac1: SVector<Real, ANG_DIM>,
         ang_jac2: SVector<Real, ANG_DIM>,
-    ) -> JointGenericVelocityGroundConstraint {
+    ) -> JointGenericOneBodyConstraint {
         let ndofs2 = mb2.ndofs();
 
-        let vel1 = lin_jac.dot(&body1.linvel) + ang_jac1.gdot(body1.angvel);
-
+        let proj_vel1 = lin_jac.dot(&body1.linvel) + ang_jac1.gdot(body1.angvel);
         let j_id2 = *j_id;
-        let vel2 = mb2
-            .fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
-            .1;
-        let rhs_wo_bias = vel2 - vel1;
+        mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians);
+        let rhs_wo_bias = -proj_vel1;
 
-        let mj_lambda2 = mb2.solver_id;
+        let solver_vel2 = mb2.solver_id;
 
-        JointGenericVelocityGroundConstraint {
-            mj_lambda2,
+        JointGenericOneBodyConstraint {
+            solver_vel2,
             ndofs2,
             j_id2,
             joint_id,
@@ -548,23 +959,22 @@ impl JointVelocityConstraintBuilder<Real> {
         }
     }
 
-    pub fn lock_linear_generic_ground(
+    pub fn lock_linear_generic_one_body(
         &self,
         params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
+        body1: &JointFixedSolverBody<Real>,
         mb2: (&Multibody, usize),
         locked_axis: usize,
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityGroundConstraint {
+    ) -> JointGenericOneBodyConstraint {
         let lin_jac = self.basis.column(locked_axis).into_owned();
         let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
         let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
 
-        let mut c = self.lock_jacobians_generic_ground(
-            params,
+        let mut c = self.lock_jacobians_generic_one_body(
             jacobians,
             j_id,
             joint_id,
@@ -582,24 +992,23 @@ impl JointVelocityConstraintBuilder<Real> {
         c
     }
 
-    pub fn limit_linear_generic_ground(
+    pub fn limit_linear_generic_one_body(
         &self,
         params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
+        body1: &JointFixedSolverBody<Real>,
         mb2: (&Multibody, usize),
         limited_axis: usize,
         limits: [Real; 2],
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityGroundConstraint {
+    ) -> JointGenericOneBodyConstraint {
         let lin_jac = self.basis.column(limited_axis).into_owned();
         let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned();
         let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned();
 
-        let mut constraint = self.lock_jacobians_generic_ground(
-            params,
+        let mut constraint = self.lock_jacobians_generic_one_body(
             jacobians,
             j_id,
             joint_id,
@@ -626,19 +1035,17 @@ impl JointVelocityConstraintBuilder<Real> {
         constraint
     }
 
-    pub fn motor_linear_generic_ground(
+    pub fn motor_linear_generic_one_body(
         &self,
-        params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>, // TODO: we shouldn’t need this.
+        body1: &JointFixedSolverBody<Real>,
         mb2: (&Multibody, usize),
         motor_axis: usize,
         motor_params: &MotorParameters<Real>,
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityGroundConstraint {
+    ) -> JointGenericOneBodyConstraint {
         let lin_jac = self.basis.column(motor_axis).into_owned();
         let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
         let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
@@ -651,8 +1058,7 @@ impl JointVelocityConstraintBuilder<Real> {
         //     constraint.ang_jac2.fill(0.0);
         // }
 
-        let mut constraint = self.lock_jacobians_generic_ground(
-            params,
+        let mut constraint = self.lock_jacobians_generic_one_body(
             jacobians,
             j_id,
             joint_id,
@@ -670,9 +1076,8 @@ impl JointVelocityConstraintBuilder<Real> {
             rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
         }
 
-        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
-            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-        rhs_wo_bias += dvel - motor_params.target_vel;
+        let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel);
+        rhs_wo_bias += proj_vel1 - motor_params.target_vel;
 
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint.rhs = rhs_wo_bias;
@@ -682,24 +1087,23 @@ impl JointVelocityConstraintBuilder<Real> {
         constraint
     }
 
-    pub fn lock_angular_generic_ground(
+    pub fn lock_angular_generic_one_body(
         &self,
         params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
+        body1: &JointFixedSolverBody<Real>,
         mb2: (&Multibody, usize),
         _locked_axis: usize,
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityGroundConstraint {
+    ) -> JointGenericOneBodyConstraint {
         #[cfg(feature = "dim2")]
         let ang_jac = Vector1::new(1.0);
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
 
-        let mut constraint = self.lock_jacobians_generic_ground(
-            params,
+        let mut constraint = self.lock_jacobians_generic_one_body(
             jacobians,
             j_id,
             joint_id,
@@ -720,25 +1124,24 @@ impl JointVelocityConstraintBuilder<Real> {
         constraint
     }
 
-    pub fn limit_angular_generic_ground(
+    pub fn limit_angular_generic_one_body(
         &self,
         params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
+        body1: &JointFixedSolverBody<Real>,
         mb2: (&Multibody, usize),
         _limited_axis: usize,
         limits: [Real; 2],
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityGroundConstraint {
+    ) -> JointGenericOneBodyConstraint {
         #[cfg(feature = "dim2")]
         let ang_jac = Vector1::new(1.0);
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
 
-        let mut constraint = self.lock_jacobians_generic_ground(
-            params,
+        let mut constraint = self.lock_jacobians_generic_one_body(
             jacobians,
             j_id,
             joint_id,
@@ -771,26 +1174,23 @@ impl JointVelocityConstraintBuilder<Real> {
         constraint
     }
 
-    pub fn motor_angular_generic_ground(
+    pub fn motor_angular_generic_one_body(
         &self,
-        params: &IntegrationParameters,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>, // TODO: we shouldn’t need this.
+        body1: &JointFixedSolverBody<Real>,
         mb2: (&Multibody, usize),
         _motor_axis: usize,
         motor_params: &MotorParameters<Real>,
         writeback_id: WritebackId,
-    ) -> JointGenericVelocityGroundConstraint {
+    ) -> JointGenericOneBodyConstraint {
         #[cfg(feature = "dim2")]
         let ang_jac = na::Vector1::new(1.0);
         #[cfg(feature = "dim3")]
         let ang_jac = self.basis.column(_motor_axis).into_owned();
 
-        let mut constraint = self.lock_jacobians_generic_ground(
-            params,
+        let mut constraint = self.lock_jacobians_generic_one_body(
             jacobians,
             j_id,
             joint_id,
@@ -813,8 +1213,8 @@ impl JointVelocityConstraintBuilder<Real> {
                 * motor_params.erp_inv_dt;
         }
 
-        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        rhs += dvel - motor_params.target_vel;
+        let proj_vel1 = -ang_jac.gdot(body1.angvel);
+        rhs += proj_vel1 - motor_params.target_vel;
 
         constraint.rhs_wo_bias = rhs;
         constraint.rhs = rhs;
@@ -824,9 +1224,9 @@ impl JointVelocityConstraintBuilder<Real> {
         constraint
     }
 
-    pub fn finalize_generic_constraints_ground(
+    pub fn finalize_generic_constraints_one_body(
         jacobians: &mut DVector<Real>,
-        constraints: &mut [JointGenericVelocityGroundConstraint],
+        constraints: &mut [JointGenericOneBodyConstraint],
     ) {
         // TODO: orthogonalization doesn’t seem to give good results for multibodies?
         const ORTHOGONALIZE: bool = false;

src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs

@@ -1,10 +1,11 @@
-use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder;
-use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::solver::joint_constraint::JointTwoBodyConstraintHelper;
+use crate::dynamics::solver::SolverVel;
 use crate::dynamics::{
     GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex,
 };
 use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
-use crate::utils::{WDot, WReal};
+use crate::num::Zero;
+use crate::utils::{SimdDot, SimdRealCopy};
 
 #[cfg(feature = "simd-is-enabled")]
 use {
@@ -13,7 +14,7 @@ use {
 };
 
 #[derive(Copy, Clone, PartialEq, Debug)]
-pub struct MotorParameters<N: WReal> {
+pub struct MotorParameters<N: SimdRealCopy> {
     pub erp_inv_dt: N,
     pub cfm_coeff: N,
     pub cfm_gain: N,
@@ -22,7 +23,7 @@ pub struct MotorParameters<N: WReal> {
     pub max_impulse: N,
 }
 
-impl<N: WReal> Default for MotorParameters<N> {
+impl<N: SimdRealCopy> Default for MotorParameters<N> {
     fn default() -> Self {
         Self {
             erp_inv_dt: N::zero(),
@@ -47,19 +48,46 @@ pub enum WritebackId {
 // the solver, to avoid fetching data from the rigid-body set
 // every time.
 #[derive(Copy, Clone)]
-pub struct SolverBody<N: WReal, const LANES: usize> {
-    pub linvel: Vector<N>,
-    pub angvel: AngVector<N>,
+pub struct JointSolverBody<N: SimdRealCopy, const LANES: usize> {
     pub im: Vector<N>,
     pub sqrt_ii: AngularInertia<N>,
     pub world_com: Point<N>,
-    pub mj_lambda: [usize; LANES],
+    pub solver_vel: [usize; LANES],
+}
+
+impl<N: SimdRealCopy, const LANES: usize> JointSolverBody<N, LANES> {
+    pub fn invalid() -> Self {
+        Self {
+            im: Vector::zeros(),
+            sqrt_ii: AngularInertia::zero(),
+            world_com: Point::origin(),
+            solver_vel: [usize::MAX; LANES],
+        }
+    }
+}
+
+#[derive(Copy, Clone)]
+pub struct JointFixedSolverBody<N: SimdRealCopy> {
+    pub linvel: Vector<N>,
+    pub angvel: AngVector<N>,
+    // TODO: is this really needed?
+    pub world_com: Point<N>,
+}
+
+impl<N: SimdRealCopy> JointFixedSolverBody<N> {
+    pub fn invalid() -> Self {
+        Self {
+            linvel: Vector::zeros(),
+            angvel: AngVector::zero(),
+            world_com: Point::origin(),
+        }
+    }
 }
 
 #[derive(Debug, Copy, Clone)]
-pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
-    pub mj_lambda1: [usize; LANES],
-    pub mj_lambda2: [usize; LANES],
+pub struct JointTwoBodyConstraint<N: SimdRealCopy, const LANES: usize> {
+    pub solver_vel1: [usize; LANES],
+    pub solver_vel2: [usize; LANES],
 
     pub joint_id: [JointIndex; LANES],
 
@@ -81,32 +109,15 @@ pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
     pub writeback_id: WritebackId,
 }
 
-impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
-    pub fn invalid() -> Self {
-        Self {
-            mj_lambda1: [crate::INVALID_USIZE; LANES],
-            mj_lambda2: [crate::INVALID_USIZE; LANES],
-            joint_id: [crate::INVALID_USIZE; LANES],
-            impulse: N::zero(),
-            impulse_bounds: [N::zero(), N::zero()],
-            lin_jac: Vector::zeros(),
-            ang_jac1: na::zero(),
-            ang_jac2: na::zero(),
-            inv_lhs: N::zero(),
-            cfm_gain: N::zero(),
-            cfm_coeff: N::zero(),
-            rhs: N::zero(),
-            rhs_wo_bias: N::zero(),
-            im1: na::zero(),
-            im2: na::zero(),
-            writeback_id: WritebackId::Dof(0),
-        }
-    }
-
-    pub fn solve_generic(&mut self, mj_lambda1: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>) {
-        let dlinvel = self.lin_jac.dot(&(mj_lambda2.linear - mj_lambda1.linear));
+impl<N: SimdRealCopy, const LANES: usize> JointTwoBodyConstraint<N, LANES> {
+    pub fn solve_generic(
+        &mut self,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) {
+        let dlinvel = self.lin_jac.dot(&(solver_vel2.linear - solver_vel1.linear));
         let dangvel =
-            self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular);
+            self.ang_jac2.gdot(solver_vel2.angular) - self.ang_jac1.gdot(solver_vel1.angular);
 
         let rhs = dlinvel + dangvel + self.rhs;
         let total_impulse = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse))
@@ -118,10 +129,10 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
         let ang_impulse1 = self.ang_jac1 * delta_impulse;
         let ang_impulse2 = self.ang_jac2 * delta_impulse;
 
-        mj_lambda1.linear += lin_impulse.component_mul(&self.im1);
-        mj_lambda1.angular += ang_impulse1;
-        mj_lambda2.linear -= lin_impulse.component_mul(&self.im2);
-        mj_lambda2.angular -= ang_impulse2;
+        solver_vel1.linear += lin_impulse.component_mul(&self.im1);
+        solver_vel1.angular += ang_impulse1;
+        solver_vel2.linear -= lin_impulse.component_mul(&self.im2);
+        solver_vel2.angular -= ang_impulse2;
     }
 
     pub fn remove_bias_from_rhs(&mut self) {
@@ -129,12 +140,12 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
     }
 }
 
-impl JointVelocityConstraint<Real, 1> {
+impl JointTwoBodyConstraint<Real, 1> {
     pub fn lock_axes(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
         joint: &GenericJoint,
@@ -146,7 +157,18 @@ impl JointVelocityConstraint<Real, 1> {
         let limit_axes = joint.limit_axes.bits() & !locked_axes;
         let coupled_axes = joint.coupled_axes.bits();
 
-        let builder = JointVelocityConstraintBuilder::new(
+        // The has_lin/ang_coupling test is needed to avoid shl overflow later.
+        let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0;
+        let first_coupled_lin_axis_id =
+            (coupled_axes & JointAxesMask::LIN_AXES.bits()).trailing_zeros() as usize;
+
+        #[cfg(feature = "dim3")]
+        let has_ang_coupling = (coupled_axes & JointAxesMask::ANG_AXES.bits()) != 0;
+        #[cfg(feature = "dim3")]
+        let first_coupled_ang_axis_id =
+            (coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize;
+
+        let builder = JointTwoBodyConstraintHelper::new(
             frame1,
             frame2,
             &body1.world_com,
@@ -195,20 +217,31 @@ impl JointVelocityConstraint<Real, 1> {
         }
 
         if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
-            // TODO: coupled linear motor constraint.
-            // out[len] = builder.motor_linear_coupled(
-            //     params,
-            //     [joint_id],
-            //     body1,
-            //     body2,
-            //     limit_axes & coupled_axes,
-            //     &joint.limits,
-            //     WritebackId::Limit(0), // TODO: writeback
-            // );
-            // len += 1;
+            // if (motor_axes & !coupled_axes) & (1 << first_coupled_lin_axis_id) != 0 {
+            //     let limits = if limit_axes & (1 << first_coupled_lin_axis_id) != 0 {
+            //         Some([
+            //             joint.limits[first_coupled_lin_axis_id].min,
+            //             joint.limits[first_coupled_lin_axis_id].max,
+            //         ])
+            //     } else {
+            //         None
+            //     };
+            //
+            //     out[len] = builder.motor_linear_coupled
+            //         params,
+            //         [joint_id],
+            //         body1,
+            //         body2,
+            //         coupled_axes,
+            //         &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
+            //         limits,
+            //         WritebackId::Motor(first_coupled_lin_axis_id),
+            //     );
+            //     len += 1;
+            // }
         }
 
-        JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
+        JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]);
 
         let start = len;
         for i in DIM..SPATIAL_DIM {
@@ -262,44 +295,50 @@ impl JointVelocityConstraint<Real, 1> {
         }
 
         #[cfg(feature = "dim3")]
-        if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
+        if has_ang_coupling && (limit_axes & (1 << first_coupled_ang_axis_id)) != 0 {
             out[len] = builder.limit_angular_coupled(
                 params,
                 [joint_id],
                 body1,
                 body2,
-                limit_axes & coupled_axes,
-                &joint.limits,
-                WritebackId::Limit(0), // TODO: writeback
+                coupled_axes,
+                [
+                    joint.limits[first_coupled_ang_axis_id].min,
+                    joint.limits[first_coupled_ang_axis_id].max,
+                ],
+                WritebackId::Limit(first_coupled_ang_axis_id),
             );
             len += 1;
         }
 
-        if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
+        if has_lin_coupling && (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
             out[len] = builder.limit_linear_coupled(
                 params,
                 [joint_id],
                 body1,
                 body2,
-                limit_axes & coupled_axes,
-                &joint.limits,
-                WritebackId::Limit(0), // TODO: writeback
+                coupled_axes,
+                [
+                    joint.limits[first_coupled_lin_axis_id].min,
+                    joint.limits[first_coupled_lin_axis_id].max,
+                ],
+                WritebackId::Limit(first_coupled_lin_axis_id),
             );
             len += 1;
         }
-        JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
+        JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]);
 
         len
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1[0] as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2[0] as usize];
+    pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+        let mut solver_vel1 = solver_vels[self.solver_vel1[0] as usize];
+        let mut solver_vel2 = solver_vels[self.solver_vel2[0] as usize];
 
-        self.solve_generic(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_generic(&mut solver_vel1, &mut solver_vel2);
 
-        mj_lambdas[self.mj_lambda1[0] as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2[0] as usize] = mj_lambda2;
+        solver_vels[self.solver_vel1[0] as usize] = solver_vel1;
+        solver_vels[self.solver_vel2[0] as usize] = solver_vel2;
     }
 
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
@@ -312,18 +351,18 @@ impl JointVelocityConstraint<Real, 1> {
     }
 }
 #[cfg(feature = "simd-is-enabled")]
-impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
+impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
     pub fn lock_axes(
         params: &IntegrationParameters,
         joint_id: [JointIndex; SIMD_WIDTH],
-        body1: &SolverBody<SimdReal, SIMD_WIDTH>,
-        body2: &SolverBody<SimdReal, SIMD_WIDTH>,
+        body1: &JointSolverBody<SimdReal, SIMD_WIDTH>,
+        body2: &JointSolverBody<SimdReal, SIMD_WIDTH>,
         frame1: &Isometry<SimdReal>,
         frame2: &Isometry<SimdReal>,
         locked_axes: u8,
         out: &mut [Self],
     ) -> usize {
-        let builder = JointVelocityConstraintBuilder::new(
+        let builder = JointTwoBodyConstraintHelper::new(
             frame1,
             frame2,
             &body1.world_com,
@@ -354,31 +393,35 @@ impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
             }
         }
 
-        JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]);
+        JointTwoBodyConstraintHelper::finalize_constraints(&mut out[..len]);
         len
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
+    pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+        let mut solver_vel1 = SolverVel {
+            linear: Vector::from(gather![
+                |ii| solver_vels[self.solver_vel1[ii] as usize].linear
+            ]),
             angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
+                |ii| solver_vels[self.solver_vel1[ii] as usize].angular
             ]),
         };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
+        let mut solver_vel2 = SolverVel {
+            linear: Vector::from(gather![
+                |ii| solver_vels[self.solver_vel2[ii] as usize].linear
+            ]),
             angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
+                |ii| solver_vels[self.solver_vel2[ii] as usize].angular
             ]),
         };
 
-        self.solve_generic(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_generic(&mut solver_vel1, &mut solver_vel2);
 
         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);
-            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);
+            solver_vels[self.solver_vel1[ii] as usize].linear = solver_vel1.linear.extract(ii);
+            solver_vels[self.solver_vel1[ii] as usize].angular = solver_vel1.angular.extract(ii);
+            solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii);
+            solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii);
         }
     }
 
@@ -398,8 +441,8 @@ impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
 }
 
 #[derive(Debug, Copy, Clone)]
-pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
-    pub mj_lambda2: [usize; LANES],
+pub struct JointOneBodyConstraint<N: SimdRealCopy, const LANES: usize> {
+    pub solver_vel2: [usize; LANES],
 
     pub joint_id: [JointIndex; LANES],
 
@@ -419,28 +462,10 @@ pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
     pub writeback_id: WritebackId,
 }
 
-impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
-    pub fn invalid() -> Self {
-        Self {
-            mj_lambda2: [crate::INVALID_USIZE; LANES],
-            joint_id: [crate::INVALID_USIZE; LANES],
-            impulse: N::zero(),
-            impulse_bounds: [N::zero(), N::zero()],
-            lin_jac: Vector::zeros(),
-            ang_jac2: na::zero(),
-            inv_lhs: N::zero(),
-            cfm_coeff: N::zero(),
-            cfm_gain: N::zero(),
-            rhs: N::zero(),
-            rhs_wo_bias: N::zero(),
-            im2: na::zero(),
-            writeback_id: WritebackId::Dof(0),
-        }
-    }
-
-    pub fn solve_generic(&mut self, mj_lambda2: &mut DeltaVel<N>) {
-        let dlinvel = mj_lambda2.linear;
-        let dangvel = mj_lambda2.angular;
+impl<N: SimdRealCopy, const LANES: usize> JointOneBodyConstraint<N, LANES> {
+    pub fn solve_generic(&mut self, solver_vel2: &mut SolverVel<N>) {
+        let dlinvel = solver_vel2.linear;
+        let dangvel = solver_vel2.angular;
 
         let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs;
         let total_impulse = (self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse))
@@ -451,8 +476,8 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
         let lin_impulse = self.lin_jac * delta_impulse;
         let ang_impulse = self.ang_jac2 * delta_impulse;
 
-        mj_lambda2.linear -= lin_impulse.component_mul(&self.im2);
-        mj_lambda2.angular -= ang_impulse;
+        solver_vel2.linear -= lin_impulse.component_mul(&self.im2);
+        solver_vel2.angular -= ang_impulse;
     }
 
     pub fn remove_bias_from_rhs(&mut self) {
@@ -460,12 +485,12 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
     }
 }
 
-impl JointVelocityGroundConstraint<Real, 1> {
+impl JointOneBodyConstraint<Real, 1> {
     pub fn lock_axes(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        body1: &SolverBody<Real, 1>,
-        body2: &SolverBody<Real, 1>,
+        body1: &JointFixedSolverBody<Real>,
+        body2: &JointSolverBody<Real, 1>,
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
         joint: &GenericJoint,
@@ -477,7 +502,18 @@ impl JointVelocityGroundConstraint<Real, 1> {
         let limit_axes = joint.limit_axes.bits() & !locked_axes;
         let coupled_axes = joint.coupled_axes.bits();
 
-        let builder = JointVelocityConstraintBuilder::new(
+        // The has_lin/ang_coupling test is needed to avoid shl overflow later.
+        let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0;
+        let first_coupled_lin_axis_id =
+            (coupled_axes & JointAxesMask::LIN_AXES.bits()).trailing_zeros() as usize;
+
+        #[cfg(feature = "dim3")]
+        let has_ang_coupling = (coupled_axes & JointAxesMask::ANG_AXES.bits()) != 0;
+        #[cfg(feature = "dim3")]
+        let first_coupled_ang_axis_id =
+            (coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize;
+
+        let builder = JointTwoBodyConstraintHelper::new(
             frame1,
             frame2,
             &body1.world_com,
@@ -488,7 +524,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
         let start = len;
         for i in DIM..SPATIAL_DIM {
             if (motor_axes & !coupled_axes) & (1 << i) != 0 {
-                out[len] = builder.motor_angular_ground(
+                out[len] = builder.motor_angular_one_body(
                     [joint_id],
                     body1,
                     body2,
@@ -507,7 +543,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
                     None
                 };
 
-                out[len] = builder.motor_linear_ground(
+                out[len] = builder.motor_linear_one_body(
                     params,
                     [joint_id],
                     body1,
@@ -521,35 +557,40 @@ impl JointVelocityGroundConstraint<Real, 1> {
             }
         }
 
-        if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
+        #[cfg(feature = "dim3")]
+        if has_ang_coupling && (motor_axes & (1 << first_coupled_ang_axis_id)) != 0 {
             // TODO: coupled angular motor constraint.
         }
 
-        if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
-            /*
-            // TODO: coupled linear motor constraint.
-            out[len] = builder.motor_linear_coupled_ground(
+        if has_lin_coupling && (motor_axes & (1 << first_coupled_lin_axis_id)) != 0 {
+            let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
+                Some([
+                    joint.limits[first_coupled_lin_axis_id].min,
+                    joint.limits[first_coupled_lin_axis_id].max,
+                ])
+            } else {
+                None
+            };
+
+            out[len] = builder.motor_linear_coupled_one_body(
                 params,
                 [joint_id],
                 body1,
                 body2,
-                motor_axes & coupled_axes,
-                &joint.motors,
-                limit_axes & coupled_axes,
-                &joint.limits,
-                WritebackId::Limit(0), // TODO: writeback
+                coupled_axes,
+                &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
+                limits,
+                WritebackId::Motor(first_coupled_lin_axis_id),
             );
             len += 1;
-            */
-            todo!()
         }
 
-        JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
+        JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]);
 
         let start = len;
         for i in DIM..SPATIAL_DIM {
             if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_angular_ground(
+                out[len] = builder.lock_angular_one_body(
                     params,
                     [joint_id],
                     body1,
@@ -562,7 +603,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
         }
         for i in 0..DIM {
             if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_linear_ground(
+                out[len] = builder.lock_linear_one_body(
                     params,
                     [joint_id],
                     body1,
@@ -576,7 +617,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
 
         for i in DIM..SPATIAL_DIM {
             if (limit_axes & !coupled_axes) & (1 << i) != 0 {
-                out[len] = builder.limit_angular_ground(
+                out[len] = builder.limit_angular_one_body(
                     params,
                     [joint_id],
                     body1,
@@ -590,7 +631,7 @@ impl JointVelocityGroundConstraint<Real, 1> {
         }
         for i in 0..DIM {
             if (limit_axes & !coupled_axes) & (1 << i) != 0 {
-                out[len] = builder.limit_linear_ground(
+                out[len] = builder.limit_linear_one_body(
                     params,
                     [joint_id],
                     body1,
@@ -604,40 +645,46 @@ impl JointVelocityGroundConstraint<Real, 1> {
         }
 
         #[cfg(feature = "dim3")]
-        if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
-            out[len] = builder.limit_angular_coupled_ground(
+        if has_ang_coupling && (limit_axes & (1 << first_coupled_ang_axis_id)) != 0 {
+            out[len] = builder.limit_angular_coupled_one_body(
                 params,
                 [joint_id],
                 body1,
                 body2,
-                limit_axes & coupled_axes,
-                &joint.limits,
-                WritebackId::Limit(0), // TODO: writeback
+                coupled_axes,
+                [
+                    joint.limits[first_coupled_ang_axis_id].min,
+                    joint.limits[first_coupled_ang_axis_id].max,
+                ],
+                WritebackId::Limit(first_coupled_ang_axis_id),
             );
             len += 1;
         }
 
-        if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
-            out[len] = builder.limit_linear_coupled_ground(
+        if has_lin_coupling && (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
+            out[len] = builder.limit_linear_coupled_one_body(
                 params,
                 [joint_id],
                 body1,
                 body2,
-                limit_axes & coupled_axes,
-                &joint.limits,
-                WritebackId::Limit(0), // TODO: writeback
+                coupled_axes,
+                [
+                    joint.limits[first_coupled_lin_axis_id].min,
+                    joint.limits[first_coupled_lin_axis_id].max,
+                ],
+                WritebackId::Limit(first_coupled_lin_axis_id),
             );
             len += 1;
         }
-        JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
+        JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]);
 
         len
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2[0] as usize];
-        self.solve_generic(&mut mj_lambda2);
-        mj_lambdas[self.mj_lambda2[0] as usize] = mj_lambda2;
+    pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+        let mut solver_vel2 = solver_vels[self.solver_vel2[0] as usize];
+        self.solve_generic(&mut solver_vel2);
+        solver_vels[self.solver_vel2[0] as usize] = solver_vel2;
     }
 
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
@@ -651,19 +698,19 @@ impl JointVelocityGroundConstraint<Real, 1> {
 }
 
 #[cfg(feature = "simd-is-enabled")]
-impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
+impl JointOneBodyConstraint<SimdReal, SIMD_WIDTH> {
     pub fn lock_axes(
         params: &IntegrationParameters,
         joint_id: [JointIndex; SIMD_WIDTH],
-        body1: &SolverBody<SimdReal, SIMD_WIDTH>,
-        body2: &SolverBody<SimdReal, SIMD_WIDTH>,
+        body1: &JointFixedSolverBody<SimdReal>,
+        body2: &JointSolverBody<SimdReal, SIMD_WIDTH>,
         frame1: &Isometry<SimdReal>,
         frame2: &Isometry<SimdReal>,
         locked_axes: u8,
         out: &mut [Self],
     ) -> usize {
         let mut len = 0;
-        let builder = JointVelocityConstraintBuilder::new(
+        let builder = JointTwoBodyConstraintHelper::new(
             frame1,
             frame2,
             &body1.world_com,
@@ -673,7 +720,7 @@ impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
 
         for i in 0..DIM {
             if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_linear_ground(
+                out[len] = builder.lock_linear_one_body(
                     params,
                     joint_id,
                     body1,
@@ -686,7 +733,7 @@ impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
         }
         for i in DIM..SPATIAL_DIM {
             if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_angular_ground(
+                out[len] = builder.lock_angular_one_body(
                     params,
                     joint_id,
                     body1,
@@ -698,23 +745,25 @@ impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
             }
         }
 
-        JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]);
+        JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[..len]);
         len
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
+    pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+        let mut solver_vel2 = SolverVel {
+            linear: Vector::from(gather![
+                |ii| solver_vels[self.solver_vel2[ii] as usize].linear
+            ]),
             angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
+                |ii| solver_vels[self.solver_vel2[ii] as usize].angular
             ]),
         };
 
-        self.solve_generic(&mut mj_lambda2);
+        self.solve_generic(&mut solver_vel2);
 
         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);
+            solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii);
+            solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii);
         }
     }
 

src/dynamics/solver/joint_constraint/mod.rs

@@ -1,13 +1,18 @@
-pub use joint_velocity_constraint::{MotorParameters, SolverBody, WritebackId};
+pub use joint_velocity_constraint::{JointSolverBody, MotorParameters, WritebackId};
 
-pub use joint_constraint::AnyJointVelocityConstraint;
-pub use joint_generic_velocity_constraint::{
-    JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
+pub use any_joint_constraint::JointConstraintTypes;
+pub use joint_constraint_builder::JointTwoBodyConstraintHelper;
+pub use joint_constraints_set::JointConstraintsSet;
+pub use joint_generic_constraint::{JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint};
+pub use joint_generic_constraint_builder::{
+    JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder,
+    JointGenericVelocityOneBodyExternalConstraintBuilder,
+    JointGenericVelocityOneBodyInternalConstraintBuilder,
 };
-pub use joint_velocity_constraint_builder::JointVelocityConstraintBuilder;
 
-mod joint_constraint;
-mod joint_generic_velocity_constraint;
-mod joint_generic_velocity_constraint_builder;
+mod any_joint_constraint;
+mod joint_constraint_builder;
+mod joint_constraints_set;
+mod joint_generic_constraint;
+mod joint_generic_constraint_builder;
 mod joint_velocity_constraint;
-mod joint_velocity_constraint_builder;

src/dynamics/solver/mod.rs

@@ -1,56 +1,47 @@
-#[cfg(not(feature = "parallel"))]
+// #[cfg(not(feature = "parallel"))]
 pub(crate) use self::island_solver::IslandSolver;
-#[cfg(feature = "parallel")]
-pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext};
-#[cfg(feature = "parallel")]
-pub(self) use self::parallel_solver_constraints::ParallelSolverConstraints;
-#[cfg(feature = "parallel")]
-pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
-#[cfg(not(feature = "parallel"))]
-pub(self) use self::solver_constraints::SolverConstraints;
-#[cfg(not(feature = "parallel"))]
+// #[cfg(feature = "parallel")]
+// pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext};
+// #[cfg(feature = "parallel")]
+// pub(self) use self::parallel_solver_constraints::ParallelSolverConstraints;
+// #[cfg(feature = "parallel")]
+// pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
+// #[cfg(not(feature = "parallel"))]
+pub(self) use self::solver_constraints_set::SolverConstraintsSet;
+// #[cfg(not(feature = "parallel"))]
 pub(self) use self::velocity_solver::VelocitySolver;
-pub(self) use delta_vel::DeltaVel;
-pub(self) use generic_velocity_constraint::*;
-pub(self) use generic_velocity_constraint_element::*;
-pub(self) use generic_velocity_ground_constraint::*;
+
+pub(self) use contact_constraint::*;
 pub(self) use interaction_groups::*;
 pub(crate) use joint_constraint::MotorParameters;
 pub use joint_constraint::*;
-pub(self) use velocity_constraint::*;
-pub(self) use velocity_constraint_element::*;
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use velocity_constraint_wide::*;
-pub(self) use velocity_ground_constraint::*;
-pub(self) use velocity_ground_constraint_element::*;
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use velocity_ground_constraint_wide::*;
+pub(self) use solver_body::SolverBody;
+pub(self) use solver_constraints_set::{AnyConstraintMut, ConstraintTypes};
+pub(self) use solver_vel::SolverVel;
 
 mod categorization;
-mod delta_vel;
-mod generic_velocity_constraint;
-mod generic_velocity_constraint_element;
-mod generic_velocity_ground_constraint;
-mod generic_velocity_ground_constraint_element;
+mod contact_constraint;
 mod interaction_groups;
-#[cfg(not(feature = "parallel"))]
+// #[cfg(not(feature = "parallel"))]
 mod island_solver;
 mod joint_constraint;
-#[cfg(feature = "parallel")]
-mod parallel_island_solver;
-#[cfg(feature = "parallel")]
-mod parallel_solver_constraints;
-#[cfg(feature = "parallel")]
-mod parallel_velocity_solver;
-#[cfg(not(feature = "parallel"))]
-mod solver_constraints;
-mod velocity_constraint;
-mod velocity_constraint_element;
-#[cfg(feature = "simd-is-enabled")]
-mod velocity_constraint_wide;
-mod velocity_ground_constraint;
-mod velocity_ground_constraint_element;
-#[cfg(feature = "simd-is-enabled")]
-mod velocity_ground_constraint_wide;
-#[cfg(not(feature = "parallel"))]
+// #[cfg(feature = "parallel")]
+// mod parallel_island_solver;
+// #[cfg(feature = "parallel")]
+// mod parallel_solver_constraints;
+// #[cfg(feature = "parallel")]
+// mod parallel_velocity_solver;
+mod solver_body;
+// #[cfg(not(feature = "parallel"))]
+mod solver_constraints_set;
+mod solver_vel;
+// #[cfg(not(feature = "parallel"))]
 mod velocity_solver;
+
+// TODO: SAFETY: restrict with bytemuck::AnyBitPattern to make this safe.
+pub unsafe fn reset_buffer<T>(buffer: &mut Vec<T>, len: usize) {
+    buffer.clear();
+    buffer.reserve(len);
+    buffer.as_mut_ptr().write_bytes(u8::MAX, len);
+    buffer.set_len(len);
+}

src/dynamics/solver/parallel_island_solver.rs

@@ -3,7 +3,7 @@ use std::sync::atomic::{AtomicUsize, Ordering};
 use rayon::Scope;
 
 use crate::dynamics::solver::{
-    AnyJointVelocityConstraint, AnyVelocityConstraint, ParallelSolverConstraints,
+    ContactConstraintTypes, JointConstraintTypes, ParallelSolverConstraints,
 };
 use crate::dynamics::{
     IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
@@ -12,7 +12,7 @@ use crate::dynamics::{
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use na::DVector;
 
-use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
+use super::{ParallelInteractionGroups, ParallelVelocitySolver, SolverVel};
 
 #[macro_export]
 #[doc(hidden)]
@@ -137,8 +137,8 @@ pub struct ParallelIslandSolver {
     velocity_solver: ParallelVelocitySolver,
     parallel_groups: ParallelInteractionGroups,
     parallel_joint_groups: ParallelInteractionGroups,
-    parallel_contact_constraints: ParallelSolverConstraints<AnyVelocityConstraint>,
-    parallel_joint_constraints: ParallelSolverConstraints<AnyJointVelocityConstraint>,
+    parallel_contact_constraints: ParallelSolverConstraints<ContactConstraintTypes>,
+    parallel_joint_constraints: ParallelSolverConstraints<JointConstraintTypes>,
     thread: ThreadContext,
 }
 
@@ -247,16 +247,16 @@ impl ParallelIslandSolver {
                 }
             }
 
-            if self.velocity_solver.generic_mj_lambdas.len() < solver_id {
-                self.velocity_solver.generic_mj_lambdas = DVector::zeros(solver_id);
+            if self.velocity_solver.generic_solver_vels.len() < solver_id {
+                self.velocity_solver.generic_solver_vels = DVector::zeros(solver_id);
             } else {
-                self.velocity_solver.generic_mj_lambdas.fill(0.0);
+                self.velocity_solver.generic_solver_vels.fill(0.0);
             }
 
-            self.velocity_solver.mj_lambdas.clear();
+            self.velocity_solver.solver_vels.clear();
             self.velocity_solver
-                .mj_lambdas
-                .resize(islands.active_island(island_id).len(), DeltaVel::zero());
+                .solver_vels
+                .resize(islands.active_island(island_id).len(), SolverVel::zero());
         }
 
         for _ in 0..num_task_per_island {
@@ -286,16 +286,16 @@ impl ParallelIslandSolver {
                     unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
                 let impulse_joints: &mut Vec<JointGraphEdge> =
                     unsafe { std::mem::transmute(impulse_joints.load(Ordering::Relaxed)) };
-                let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint> = unsafe {
+                let parallel_contact_constraints: &mut ParallelSolverConstraints<ContactConstraintTypes> = unsafe {
                     std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
                 };
-                let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint> = unsafe {
+                let parallel_joint_constraints: &mut ParallelSolverConstraints<JointConstraintTypes> = unsafe {
                     std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed))
                 };
 
                 enable_flush_to_zero!(); // Ensure this is enabled on each thread.
 
-                // Initialize `mj_lambdas` (per-body velocity deltas) with external accelerations (gravity etc):
+                // Initialize `solver_vels` (per-body velocity deltas) with external accelerations (gravity etc):
                 {
                     let island_range = islands.active_island_range(island_id);
                     let active_bodies = &islands.active_dynamic_set[island_range];
@@ -309,14 +309,14 @@ impl ParallelIslandSolver {
                                     .unwrap();
 
                                 if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                                    let mut mj_lambdas = velocity_solver
-                                        .generic_mj_lambdas
+                                    let mut solver_vels = velocity_solver
+                                        .generic_solver_vels
                                         .rows_mut(multibody.solver_id, multibody.ndofs());
-                                    mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
+                                    solver_vels.axpy(params.dt, &multibody.accelerations, 0.0);
                                 }
                             } else {
                                 let rb = &bodies[*handle];
-                                let dvel = &mut velocity_solver.mj_lambdas[rb.ids.active_set_offset];
+                                let dvel = &mut velocity_solver.solver_vels[rb.ids.active_set_offset];
 
                                 // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
                                 //       by the square root of the inertia tensor:

src/dynamics/solver/parallel_solver_constraints.rs

@@ -1,10 +1,9 @@
 use super::ParallelInteractionGroups;
-use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext};
+use super::{ContactConstraintTypes, JointConstraintTypes, ThreadContext};
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
-use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint;
+use crate::dynamics::solver::generic_two_body_constraint::GenericTwoBodyConstraint;
 use crate::dynamics::solver::{
-    GenericVelocityGroundConstraint, InteractionGroups, VelocityConstraint,
-    VelocityGroundConstraint,
+    GenericOneBodyConstraint, InteractionGroups, OneBodyConstraint, TwoBodyConstraint,
 };
 use crate::dynamics::{
     ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex,
@@ -14,7 +13,7 @@ use crate::geometry::ContactManifold;
 use crate::math::{Real, SPATIAL_DIM};
 #[cfg(feature = "simd-is-enabled")]
 use crate::{
-    dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
+    dynamics::solver::{OneBodyConstraintSimd, TwoBodyConstraintSimd},
     math::SIMD_WIDTH,
 };
 use na::DVector;
@@ -36,40 +35,40 @@ use std::sync::atomic::Ordering;
 // }
 
 pub(crate) enum ConstraintDesc {
-    NongroundNongrouped(usize),
-    GroundNongrouped(usize),
+    TwoBodyNongrouped(usize),
+    OneBodyNongrouped(usize),
     #[cfg(feature = "simd-is-enabled")]
-    NongroundGrouped([usize; SIMD_WIDTH]),
+    TwoBodyGrouped([usize; SIMD_WIDTH]),
     #[cfg(feature = "simd-is-enabled")]
-    GroundGrouped([usize; SIMD_WIDTH]),
-    GenericNongroundNongrouped(usize, usize),
-    GenericGroundNongrouped(usize, usize),
+    OneBodyGrouped([usize; SIMD_WIDTH]),
+    GenericTwoBodyNongrouped(usize, usize),
+    GenericOneBodyNongrouped(usize, usize),
     GenericMultibodyInternal(MultibodyIndex, usize),
 }
 
-pub(crate) struct ParallelSolverConstraints<VelocityConstraint> {
+pub(crate) struct ParallelSolverConstraints<TwoBodyConstraint> {
     pub generic_jacobians: DVector<Real>,
-    pub not_ground_interactions: Vec<usize>,
-    pub ground_interactions: Vec<usize>,
-    pub generic_not_ground_interactions: Vec<usize>,
-    pub generic_ground_interactions: Vec<usize>,
+    pub two_body_interactions: Vec<usize>,
+    pub one_body_interactions: Vec<usize>,
+    pub generic_two_body_interactions: Vec<usize>,
+    pub generic_one_body_interactions: Vec<usize>,
     pub interaction_groups: InteractionGroups,
-    pub ground_interaction_groups: InteractionGroups,
-    pub velocity_constraints: Vec<VelocityConstraint>,
+    pub one_body_interaction_groups: InteractionGroups,
+    pub velocity_constraints: Vec<TwoBodyConstraint>,
     pub constraint_descs: Vec<(usize, ConstraintDesc)>,
     pub parallel_desc_groups: Vec<usize>,
 }
 
-impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> {
+impl<TwoBodyConstraint> ParallelSolverConstraints<TwoBodyConstraint> {
     pub fn new() -> Self {
         Self {
             generic_jacobians: DVector::zeros(0),
-            not_ground_interactions: vec![],
-            ground_interactions: vec![],
-            generic_not_ground_interactions: vec![],
-            generic_ground_interactions: vec![],
+            two_body_interactions: vec![],
+            one_body_interactions: vec![],
+            generic_two_body_interactions: vec![],
+            generic_one_body_interactions: vec![],
             interaction_groups: InteractionGroups::new(),
-            ground_interaction_groups: InteractionGroups::new(),
+            one_body_interaction_groups: InteractionGroups::new(),
             velocity_constraints: vec![],
             constraint_descs: vec![],
             parallel_desc_groups: vec![],
@@ -78,14 +77,14 @@ impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> {
 }
 
 macro_rules! impl_init_constraints_group {
-    ($VelocityConstraint: ty, $Interaction: ty,
+    ($TwoBodyConstraint: ty, $Interaction: ty,
      $categorize: ident, $group: ident,
      $body1: ident,
      $body2: ident,
      $generate_internal_constraints: expr,
      $num_active_constraints_and_jacobian_lines: path,
      $empty_velocity_constraint: expr $(, $weight: ident)*) => {
-        impl ParallelSolverConstraints<$VelocityConstraint> {
+        impl ParallelSolverConstraints<$TwoBodyConstraint> {
             pub fn init_constraint_groups(
                 &mut self,
                 island_id: usize,
@@ -100,7 +99,7 @@ macro_rules! impl_init_constraints_group {
                 let num_groups = interaction_groups.num_groups();
 
                 self.interaction_groups.clear_groups();
-                self.ground_interaction_groups.clear_groups();
+                self.one_body_interaction_groups.clear_groups();
                 self.parallel_desc_groups.clear();
                 self.constraint_descs.clear();
                 self.parallel_desc_groups.push(0);
@@ -108,43 +107,43 @@ macro_rules! impl_init_constraints_group {
                 for i in 0..num_groups {
                     let group = interaction_groups.group(i);
 
-                    self.not_ground_interactions.clear();
-                    self.ground_interactions.clear();
-                    self.generic_not_ground_interactions.clear();
-                    self.generic_ground_interactions.clear();
+                    self.two_body_interactions.clear();
+                    self.one_body_interactions.clear();
+                    self.generic_two_body_interactions.clear();
+                    self.generic_one_body_interactions.clear();
 
                     $categorize(
                         bodies,
                         multibodies,
                         interactions,
                         group,
-                        &mut self.ground_interactions,
-                        &mut self.not_ground_interactions,
-                        &mut self.generic_ground_interactions,
-                        &mut self.generic_not_ground_interactions,
+                        &mut self.one_body_interactions,
+                        &mut self.two_body_interactions,
+                        &mut self.generic_one_body_interactions,
+                        &mut self.generic_two_body_interactions,
                     );
 
                     #[cfg(feature = "simd-is-enabled")]
-                    let start_grouped = self.interaction_groups.grouped_interactions.len();
+                    let start_grouped = self.interaction_groups.simd_interactions.len();
                     let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
 
                     #[cfg(feature = "simd-is-enabled")]
-                    let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
-                    let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len();
+                    let start_grouped_one_body = self.one_body_interaction_groups.simd_interactions.len();
+                    let start_nongrouped_one_body = self.one_body_interaction_groups.nongrouped_interactions.len();
 
                     self.interaction_groups.$group(
                         island_id,
                         islands,
                         bodies,
                         interactions,
-                        &self.not_ground_interactions,
+                        &self.two_body_interactions,
                     );
-                    self.ground_interaction_groups.$group(
+                    self.one_body_interaction_groups.$group(
                         island_id,
                         islands,
                         bodies,
                         interactions,
-                        &self.ground_interactions,
+                        &self.one_body_interactions,
                     );
 
                     // Compute constraint indices.
@@ -152,19 +151,19 @@ macro_rules! impl_init_constraints_group {
                         let interaction = &mut interactions[*interaction_i]$(.$weight)*;
                         self.constraint_descs.push((
                             total_num_constraints,
-                            ConstraintDesc::NongroundNongrouped(*interaction_i),
+                            ConstraintDesc::TwoBodyNongrouped(*interaction_i),
                         ));
                         total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
                     }
 
                     #[cfg(feature = "simd-is-enabled")]
                     for interaction_i in
-                        self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
+                        self.interaction_groups.simd_interactions[start_grouped..].chunks(SIMD_WIDTH)
                     {
                         let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
                         self.constraint_descs.push((
                             total_num_constraints,
-                            ConstraintDesc::NongroundGrouped(
+                            ConstraintDesc::TwoBodyGrouped(
                                 gather![|ii| interaction_i[ii]],
                             ),
                         ));
@@ -172,25 +171,25 @@ macro_rules! impl_init_constraints_group {
                     }
 
                     for interaction_i in
-                        &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
+                        &self.one_body_interaction_groups.nongrouped_interactions[start_nongrouped_one_body..]
                     {
                         let interaction = &mut interactions[*interaction_i]$(.$weight)*;
                         self.constraint_descs.push((
                             total_num_constraints,
-                            ConstraintDesc::GroundNongrouped(*interaction_i),
+                            ConstraintDesc::OneBodyNongrouped(*interaction_i),
                         ));
                         total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
                     }
 
                     #[cfg(feature = "simd-is-enabled")]
-                    for interaction_i in self.ground_interaction_groups.grouped_interactions
-                        [start_grouped_ground..]
+                    for interaction_i in self.one_body_interaction_groups.simd_interactions
+                        [start_grouped_one_body..]
                         .chunks(SIMD_WIDTH)
                     {
                         let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
                         self.constraint_descs.push((
                             total_num_constraints,
-                            ConstraintDesc::GroundGrouped(
+                            ConstraintDesc::OneBodyGrouped(
                                 gather![|ii| interaction_i[ii]],
                             ),
                         ));
@@ -208,11 +207,11 @@ macro_rules! impl_init_constraints_group {
                         }
                     };
 
-                    for interaction_i in &self.generic_not_ground_interactions[..] {
+                    for interaction_i in &self.generic_two_body_interactions[..] {
                         let interaction = &mut interactions[*interaction_i]$(.$weight)*;
                         self.constraint_descs.push((
                             total_num_constraints,
-                            ConstraintDesc::GenericNongroundNongrouped(*interaction_i, *j_id),
+                            ConstraintDesc::GenericTwoBodyNongrouped(*interaction_i, *j_id),
                         ));
                         let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction);
                         let ndofs1 = $body1(interaction).map(multibody_ndofs).unwrap_or(0);
@@ -222,11 +221,11 @@ macro_rules! impl_init_constraints_group {
                         total_num_constraints += num_constraints;
                     }
 
-                    for interaction_i in &self.generic_ground_interactions[..] {
+                    for interaction_i in &self.generic_one_body_interactions[..] {
                         let interaction = &mut interactions[*interaction_i]$(.$weight)*;
                         self.constraint_descs.push((
                             total_num_constraints,
-                            ConstraintDesc::GenericGroundNongrouped(*interaction_i, *j_id),
+                            ConstraintDesc::GenericOneBodyNongrouped(*interaction_i, *j_id),
                         ));
 
                         let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction);
@@ -289,31 +288,31 @@ fn manifold_body2(manifold: &ContactManifold) -> Option<RigidBodyHandle> {
 }
 
 impl_init_constraints_group!(
-    AnyVelocityConstraint,
+    ContactConstraintTypes,
     &mut ContactManifold,
     categorize_contacts,
     group_manifolds,
     manifold_body1,
     manifold_body2,
     false,
-    VelocityConstraint::num_active_constraints_and_jacobian_lines,
-    AnyVelocityConstraint::Empty
+    TwoBodyConstraint::num_active_constraints_and_jacobian_lines,
+    ContactConstraintTypes::Empty
 );
 
 impl_init_constraints_group!(
-    AnyJointVelocityConstraint,
+    JointConstraintTypes,
     JointGraphEdge,
     categorize_joints,
     group_joints,
     joint_body1,
     joint_body2,
     true,
-    AnyJointVelocityConstraint::num_active_constraints_and_jacobian_lines,
-    AnyJointVelocityConstraint::Empty,
+    JointConstraintTypes::num_active_constraints_and_jacobian_lines,
+    JointConstraintTypes::Empty,
     weight
 );
 
-impl ParallelSolverConstraints<AnyVelocityConstraint> {
+impl ParallelSolverConstraints<ContactConstraintTypes> {
     pub fn fill_constraints(
         &mut self,
         thread: &ThreadContext,
@@ -328,33 +327,33 @@ impl ParallelSolverConstraints<AnyVelocityConstraint> {
             let batch_size = thread.batch_size;
             for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] {
                 match &desc.1 {
-                    ConstraintDesc::NongroundNongrouped(manifold_id) => {
+                    ConstraintDesc::TwoBodyNongrouped(manifold_id) => {
                         let manifold = &*manifolds_all[*manifold_id];
-                        VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
+                        TwoBodyConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
-                    ConstraintDesc::GroundNongrouped(manifold_id) => {
+                    ConstraintDesc::OneBodyNongrouped(manifold_id) => {
                         let manifold = &*manifolds_all[*manifold_id];
-                        VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
+                        OneBodyConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
                     #[cfg(feature = "simd-is-enabled")]
-                    ConstraintDesc::NongroundGrouped(manifold_id) => {
+                    ConstraintDesc::TwoBodyGrouped(manifold_id) => {
                         let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
-                        WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
+                        TwoBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
                     #[cfg(feature = "simd-is-enabled")]
-                    ConstraintDesc::GroundGrouped(manifold_id) => {
+                    ConstraintDesc::OneBodyGrouped(manifold_id) => {
                         let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
-                        WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
+                        OneBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
-                    ConstraintDesc::GenericNongroundNongrouped(manifold_id, j_id) => {
+                    ConstraintDesc::GenericTwoBodyNongrouped(manifold_id, j_id) => {
                         let mut j_id = *j_id;
                         let manifold = &*manifolds_all[*manifold_id];
-                        GenericVelocityConstraint::generate(params, *manifold_id, manifold, bodies, multibodies,  &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
+                        GenericTwoBodyConstraint::generate(params, *manifold_id, manifold, bodies, multibodies,  &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
                     }
-                    ConstraintDesc::GenericGroundNongrouped(manifold_id, j_id) => {
+                    ConstraintDesc::GenericOneBodyNongrouped(manifold_id, j_id) => {
                         let mut j_id = *j_id;
                         let manifold = &*manifolds_all[*manifold_id];
-                        GenericVelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
+                        GenericOneBodyConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
                     }
                     ConstraintDesc::GenericMultibodyInternal(..) => unreachable!()
                 }
@@ -363,7 +362,7 @@ impl ParallelSolverConstraints<AnyVelocityConstraint> {
     }
 }
 
-impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
+impl ParallelSolverConstraints<JointConstraintTypes> {
     pub fn fill_constraints(
         &mut self,
         thread: &ThreadContext,
@@ -378,33 +377,33 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
             let batch_size = thread.batch_size;
             for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] {
                 match &desc.1 {
-                    ConstraintDesc::NongroundNongrouped(joint_id) => {
+                    ConstraintDesc::TwoBodyNongrouped(joint_id) => {
                         let joint = &joints_all[*joint_id].weight;
-                        AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
+                        JointConstraintTypes::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
                     }
-                    ConstraintDesc::GroundNongrouped(joint_id) => {
+                    ConstraintDesc::OneBodyNongrouped(joint_id) => {
                         let joint = &joints_all[*joint_id].weight;
-                        AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
+                        JointConstraintTypes::from_joint_one_body(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
                     }
                     #[cfg(feature = "simd-is-enabled")]
-                    ConstraintDesc::NongroundGrouped(joint_id) => {
+                    ConstraintDesc::TwoBodyGrouped(joint_id) => {
                         let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
-                        AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
+                        JointConstraintTypes::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
                     #[cfg(feature = "simd-is-enabled")]
-                    ConstraintDesc::GroundGrouped(joint_id) => {
+                    ConstraintDesc::OneBodyGrouped(joint_id) => {
                         let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
-                        AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
+                        JointConstraintTypes::from_wide_joint_one_body(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
-                    ConstraintDesc::GenericNongroundNongrouped(joint_id, j_id) => {
+                    ConstraintDesc::GenericTwoBodyNongrouped(joint_id, j_id) => {
                         let mut j_id = *j_id;
                         let joint = &joints_all[*joint_id].weight;
-                        AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
+                        JointConstraintTypes::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
                     }
-                    ConstraintDesc::GenericGroundNongrouped(joint_id, j_id) => {
+                    ConstraintDesc::GenericOneBodyNongrouped(joint_id, j_id) => {
                         let mut j_id = *j_id;
                         let joint = &joints_all[*joint_id].weight;
-                        AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
+                        JointConstraintTypes::from_joint_one_body(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
                     }
                     ConstraintDesc::GenericMultibodyInternal(multibody_id, j_id) => {
                         let mut j_id = *j_id;

src/dynamics/solver/parallel_velocity_solver.rs

@@ -1,4 +1,4 @@
-use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
+use super::{ContactConstraintTypes, JointConstraintTypes, SolverVel, ThreadContext};
 use crate::concurrent_loop;
 use crate::dynamics::{
     solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge,
@@ -6,21 +6,21 @@ use crate::dynamics::{
 };
 use crate::geometry::ContactManifold;
 use crate::math::Real;
-use crate::utils::WAngularInertia;
+use crate::utils::SimdAngularInertia;
 
 use na::DVector;
 use std::sync::atomic::Ordering;
 
 pub(crate) struct ParallelVelocitySolver {
-    pub mj_lambdas: Vec<DeltaVel<Real>>,
-    pub generic_mj_lambdas: DVector<Real>,
+    pub solver_vels: Vec<SolverVel<Real>>,
+    pub generic_solver_vels: DVector<Real>,
 }
 
 impl ParallelVelocitySolver {
     pub fn new() -> Self {
         Self {
-            mj_lambdas: Vec::new(),
-            generic_mj_lambdas: DVector::zeros(0),
+            solver_vels: Vec::new(),
+            generic_solver_vels: DVector::zeros(0),
         }
     }
 
@@ -34,8 +34,8 @@ impl ParallelVelocitySolver {
         multibodies: &mut MultibodyJointSet,
         manifolds_all: &mut [&mut ContactManifold],
         joints_all: &mut [JointGraphEdge],
-        contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint>,
-        joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint>,
+        contact_constraints: &mut ParallelSolverConstraints<ContactConstraintTypes>,
+        joint_constraints: &mut ParallelSolverConstraints<JointConstraintTypes>,
     ) {
         let mut start_index = thread
             .solve_interaction_index
@@ -104,16 +104,15 @@ impl ParallelVelocitySolver {
          * Solve constraints.
          */
         {
-            for i in 0..params.max_velocity_iterations {
-                let solve_friction = params.interleave_restitution_and_friction_resolution
-                    && params.max_velocity_friction_iterations + i
-                        >= params.max_velocity_iterations;
+            for i in 0..params.num_velocity_iterations_per_small_step {
+                let solve_friction = params.num_friction_iteration_per_solver_iteration + i
+                    >= params.num_velocity_iterations_per_small_step;
                 // Solve joints.
                 solve!(
                     joint_constraints,
                     &joint_constraints.generic_jacobians,
-                    &mut self.mj_lambdas,
-                    &mut self.generic_mj_lambdas
+                    &mut self.solver_vels,
+                    &mut self.generic_solver_vels
                 );
                 shift += joint_descs.len();
                 start_index -= joint_descs.len();
@@ -122,8 +121,8 @@ impl ParallelVelocitySolver {
                 solve!(
                     contact_constraints,
                     &contact_constraints.generic_jacobians,
-                    &mut self.mj_lambdas,
-                    &mut self.generic_mj_lambdas,
+                    &mut self.solver_vels,
+                    &mut self.generic_solver_vels,
                     true,
                     false
                 );
@@ -134,8 +133,8 @@ impl ParallelVelocitySolver {
                 solve!(
                     contact_constraints,
                     &contact_constraints.generic_jacobians,
-                    &mut self.mj_lambdas,
-                    &mut self.generic_mj_lambdas,
+                    &mut self.solver_vels,
+                    &mut self.generic_solver_vels,
                     true,
                     false
                 );
@@ -146,8 +145,8 @@ impl ParallelVelocitySolver {
                     solve!(
                         contact_constraints,
                         &contact_constraints.generic_jacobians,
-                        &mut self.mj_lambdas,
-                        &mut self.generic_mj_lambdas,
+                        &mut self.solver_vels,
+                        &mut self.generic_solver_vels,
                         false,
                         true
                     );
@@ -157,21 +156,22 @@ impl ParallelVelocitySolver {
             }
 
             // Solve the remaining friction iterations.
-            let remaining_friction_iterations =
-                if !params.interleave_restitution_and_friction_resolution {
-                    params.max_velocity_friction_iterations
-                } else if params.max_velocity_friction_iterations > params.max_velocity_iterations {
-                    params.max_velocity_friction_iterations - params.max_velocity_iterations
-                } else {
-                    0
-                };
+            let remaining_friction_iterations = if params
+                .num_friction_iteration_per_solver_iteration
+                > params.num_velocity_iterations_per_small_step
+            {
+                params.num_friction_iteration_per_solver_iteration
+                    - params.num_velocity_iterations_per_small_step
+            } else {
+                0
+            };
 
             for _ in 0..remaining_friction_iterations {
                 solve!(
                     contact_constraints,
                     &contact_constraints.generic_jacobians,
-                    &mut self.mj_lambdas,
-                    &mut self.generic_mj_lambdas,
+                    &mut self.solver_vels,
+                    &mut self.generic_solver_vels,
                     false,
                     true
                 );
@@ -194,18 +194,18 @@ impl ParallelVelocitySolver {
                             .unwrap();
 
                         if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                            let mj_lambdas = self
-                                .generic_mj_lambdas
+                            let solver_vels = self
+                                .generic_solver_vels
                                 .rows(multibody.solver_id, multibody.ndofs());
                             let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations.
-                            multibody.velocities += mj_lambdas;
+                            multibody.velocities += solver_vels;
                             multibody.integrate(params.dt);
                             multibody.forward_kinematics(bodies, false);
                             multibody.velocities = prev_vels;
                         }
                     } else {
                         let rb = bodies.index_mut_internal(*handle);
-                        let dvel = self.mj_lambdas[rb.ids.active_set_offset];
+                        let dvel = self.solver_vels[rb.ids.active_set_offset];
                         let dangvel = rb.mprops
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dvel.angular);
@@ -252,8 +252,8 @@ impl ParallelVelocitySolver {
                 solve!(
                     joint_constraints,
                     &joint_constraints.generic_jacobians,
-                    &mut self.mj_lambdas,
-                    &mut self.generic_mj_lambdas
+                    &mut self.solver_vels,
+                    &mut self.generic_solver_vels
                 );
                 shift += joint_descs.len();
                 start_index -= joint_descs.len();
@@ -261,8 +261,8 @@ impl ParallelVelocitySolver {
                 solve!(
                     contact_constraints,
                     &contact_constraints.generic_jacobians,
-                    &mut self.mj_lambdas,
-                    &mut self.generic_mj_lambdas,
+                    &mut self.solver_vels,
+                    &mut self.generic_solver_vels,
                     true,
                     false
                 );
@@ -272,8 +272,8 @@ impl ParallelVelocitySolver {
                 solve!(
                     contact_constraints,
                     &contact_constraints.generic_jacobians,
-                    &mut self.mj_lambdas,
-                    &mut self.generic_mj_lambdas,
+                    &mut self.solver_vels,
+                    &mut self.generic_solver_vels,
                     false,
                     true
                 );
@@ -296,14 +296,14 @@ impl ParallelVelocitySolver {
                             .unwrap();
 
                         if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                            let mj_lambdas = self
-                                .generic_mj_lambdas
+                            let solver_vels = self
+                                .generic_solver_vels
                                 .rows(multibody.solver_id, multibody.ndofs());
-                            multibody.velocities += mj_lambdas;
+                            multibody.velocities += solver_vels;
                         }
                     } else {
                         let rb = bodies.index_mut_internal(*handle);
-                        let dvel = self.mj_lambdas[rb.ids.active_set_offset];
+                        let dvel = self.solver_vels[rb.ids.active_set_offset];
                         let dangvel = rb.mprops
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dvel.angular);

src/dynamics/solver/solver_body.rs

@@ -0,0 +1,59 @@
+use crate::dynamics::{RigidBody, RigidBodyVelocity};
+use crate::math::{AngularInertia, Isometry, Point, Real, Vector};
+use crate::prelude::RigidBodyDamping;
+
+#[cfg(feature = "dim2")]
+use crate::num::Zero;
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct SolverBody {
+    pub position: Isometry<Real>,
+    pub integrated_vels: RigidBodyVelocity,
+    pub im: Vector<Real>,
+    pub sqrt_ii: AngularInertia<Real>,
+    pub world_com: Point<Real>,
+    pub ccd_thickness: Real,
+    pub damping: RigidBodyDamping,
+    pub local_com: Point<Real>,
+}
+
+impl Default for SolverBody {
+    fn default() -> Self {
+        Self {
+            position: Isometry::identity(),
+            integrated_vels: RigidBodyVelocity::zero(),
+            im: na::zero(),
+            sqrt_ii: AngularInertia::zero(),
+            world_com: Point::origin(),
+            ccd_thickness: 0.0,
+            damping: RigidBodyDamping::default(),
+            local_com: Point::origin(),
+        }
+    }
+}
+
+impl SolverBody {
+    pub fn from(rb: &RigidBody) -> Self {
+        Self {
+            position: rb.pos.position,
+            integrated_vels: RigidBodyVelocity::zero(),
+            im: rb.mprops.effective_inv_mass,
+            sqrt_ii: rb.mprops.effective_world_inv_inertia_sqrt,
+            world_com: rb.mprops.world_com,
+            ccd_thickness: rb.ccd.ccd_thickness,
+            damping: rb.damping,
+            local_com: rb.mprops.local_mprops.local_com,
+        }
+    }
+
+    pub fn copy_from(&mut self, rb: &RigidBody) {
+        self.position = rb.pos.position;
+        self.integrated_vels = RigidBodyVelocity::zero();
+        self.im = rb.mprops.effective_inv_mass;
+        self.sqrt_ii = rb.mprops.effective_world_inv_inertia_sqrt;
+        self.world_com = rb.mprops.world_com;
+        self.ccd_thickness = rb.ccd.ccd_thickness;
+        self.damping = rb.damping;
+        self.local_com = rb.mprops.local_mprops.local_com;
+    }
+}

src/dynamics/solver/solver_constraints.rs

@@ -1,564 +0,0 @@
-use super::{
-    AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint,
-};
-#[cfg(feature = "simd-is-enabled")]
-use super::{WVelocityConstraint, WVelocityGroundConstraint};
-use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
-use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint;
-use crate::dynamics::solver::GenericVelocityConstraint;
-use crate::dynamics::{
-    solver::AnyVelocityConstraint, IntegrationParameters, IslandManager, JointGraphEdge,
-    JointIndex, MultibodyJointSet, RigidBodySet,
-};
-use crate::geometry::{ContactManifold, ContactManifoldIndex};
-use crate::math::Real;
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::SIMD_WIDTH;
-use na::DVector;
-
-pub(crate) struct SolverConstraints<VelocityConstraint> {
-    pub generic_jacobians: DVector<Real>,
-    pub not_ground_interactions: Vec<usize>,
-    pub ground_interactions: Vec<usize>,
-    pub generic_not_ground_interactions: Vec<usize>,
-    pub generic_ground_interactions: Vec<usize>,
-    pub interaction_groups: InteractionGroups,
-    pub ground_interaction_groups: InteractionGroups,
-    pub velocity_constraints: Vec<VelocityConstraint>,
-}
-
-impl<VelocityConstraint> SolverConstraints<VelocityConstraint> {
-    pub fn new() -> Self {
-        Self {
-            generic_jacobians: DVector::zeros(0),
-            not_ground_interactions: vec![],
-            ground_interactions: vec![],
-            generic_not_ground_interactions: vec![],
-            generic_ground_interactions: vec![],
-            interaction_groups: InteractionGroups::new(),
-            ground_interaction_groups: InteractionGroups::new(),
-            velocity_constraints: vec![],
-        }
-    }
-
-    // pub fn clear(&mut self) {
-    //     self.not_ground_interactions.clear();
-    //     self.ground_interactions.clear();
-    //     self.generic_not_ground_interactions.clear();
-    //     self.generic_ground_interactions.clear();
-    //     self.interaction_groups.clear();
-    //     self.ground_interaction_groups.clear();
-    //     self.velocity_constraints.clear();
-    // }
-}
-
-impl SolverConstraints<AnyVelocityConstraint> {
-    pub fn init_constraint_groups(
-        &mut self,
-        island_id: usize,
-        islands: &IslandManager,
-        bodies: &RigidBodySet,
-        multibody_joints: &MultibodyJointSet,
-        manifolds: &[&mut ContactManifold],
-        manifold_indices: &[ContactManifoldIndex],
-    ) {
-        self.not_ground_interactions.clear();
-        self.ground_interactions.clear();
-        self.generic_not_ground_interactions.clear();
-        self.generic_ground_interactions.clear();
-
-        categorize_contacts(
-            bodies,
-            multibody_joints,
-            manifolds,
-            manifold_indices,
-            &mut self.ground_interactions,
-            &mut self.not_ground_interactions,
-            &mut self.generic_ground_interactions,
-            &mut self.generic_not_ground_interactions,
-        );
-
-        self.interaction_groups.clear_groups();
-        self.interaction_groups.group_manifolds(
-            island_id,
-            islands,
-            bodies,
-            manifolds,
-            &self.not_ground_interactions,
-        );
-
-        self.ground_interaction_groups.clear_groups();
-        self.ground_interaction_groups.group_manifolds(
-            island_id,
-            islands,
-            bodies,
-            manifolds,
-            &self.ground_interactions,
-        );
-
-        // NOTE: uncomment this do disable SIMD contact resolution.
-        //        self.interaction_groups
-        //            .nongrouped_interactions
-        //            .append(&mut self.interaction_groups.grouped_interactions);
-        //        self.ground_interaction_groups
-        //            .nongrouped_interactions
-        //            .append(&mut self.ground_interaction_groups.grouped_interactions);
-    }
-
-    pub fn init(
-        &mut self,
-        island_id: usize,
-        params: &IntegrationParameters,
-        islands: &IslandManager,
-        bodies: &RigidBodySet,
-        multibody_joints: &MultibodyJointSet,
-        manifolds: &[&mut ContactManifold],
-        manifold_indices: &[ContactManifoldIndex],
-    ) {
-        self.velocity_constraints.clear();
-
-        self.init_constraint_groups(
-            island_id,
-            islands,
-            bodies,
-            multibody_joints,
-            manifolds,
-            manifold_indices,
-        );
-
-        let mut jacobian_id = 0;
-        #[cfg(feature = "simd-is-enabled")]
-        {
-            self.compute_grouped_constraints(params, bodies, manifolds);
-        }
-        self.compute_nongrouped_constraints(params, bodies, manifolds);
-        self.compute_generic_constraints(
-            params,
-            bodies,
-            multibody_joints,
-            manifolds,
-            &mut jacobian_id,
-        );
-
-        #[cfg(feature = "simd-is-enabled")]
-        {
-            self.compute_grouped_ground_constraints(params, bodies, manifolds);
-        }
-        self.compute_nongrouped_ground_constraints(params, bodies, manifolds);
-        self.compute_generic_ground_constraints(
-            params,
-            bodies,
-            multibody_joints,
-            manifolds,
-            &mut jacobian_id,
-        );
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    fn compute_grouped_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        manifolds_all: &[&mut ContactManifold],
-    ) {
-        for manifolds_i in self
-            .interaction_groups
-            .grouped_interactions
-            .chunks_exact(SIMD_WIDTH)
-        {
-            let manifold_id = gather![|ii| manifolds_i[ii]];
-            let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
-            WVelocityConstraint::generate(
-                params,
-                manifold_id,
-                manifolds,
-                bodies,
-                &mut self.velocity_constraints,
-                None,
-            );
-        }
-    }
-
-    fn compute_nongrouped_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        manifolds_all: &[&mut ContactManifold],
-    ) {
-        for manifold_i in &self.interaction_groups.nongrouped_interactions {
-            let manifold = &manifolds_all[*manifold_i];
-            VelocityConstraint::generate(
-                params,
-                *manifold_i,
-                manifold,
-                bodies,
-                &mut self.velocity_constraints,
-                None,
-            );
-        }
-    }
-
-    fn compute_generic_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        multibody_joints: &MultibodyJointSet,
-        manifolds_all: &[&mut ContactManifold],
-        jacobian_id: &mut usize,
-    ) {
-        for manifold_i in &self.generic_not_ground_interactions {
-            let manifold = &manifolds_all[*manifold_i];
-            GenericVelocityConstraint::generate(
-                params,
-                *manifold_i,
-                manifold,
-                bodies,
-                multibody_joints,
-                &mut self.velocity_constraints,
-                &mut self.generic_jacobians,
-                jacobian_id,
-                None,
-            );
-        }
-    }
-
-    fn compute_generic_ground_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        multibody_joints: &MultibodyJointSet,
-        manifolds_all: &[&mut ContactManifold],
-        jacobian_id: &mut usize,
-    ) {
-        for manifold_i in &self.generic_ground_interactions {
-            let manifold = &manifolds_all[*manifold_i];
-            GenericVelocityGroundConstraint::generate(
-                params,
-                *manifold_i,
-                manifold,
-                bodies,
-                multibody_joints,
-                &mut self.velocity_constraints,
-                &mut self.generic_jacobians,
-                jacobian_id,
-                None,
-            );
-        }
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    fn compute_grouped_ground_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        manifolds_all: &[&mut ContactManifold],
-    ) {
-        for manifolds_i in self
-            .ground_interaction_groups
-            .grouped_interactions
-            .chunks_exact(SIMD_WIDTH)
-        {
-            let manifold_id = gather![|ii| manifolds_i[ii]];
-            let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
-            WVelocityGroundConstraint::generate(
-                params,
-                manifold_id,
-                manifolds,
-                bodies,
-                &mut self.velocity_constraints,
-                None,
-            );
-        }
-    }
-
-    fn compute_nongrouped_ground_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        manifolds_all: &[&mut ContactManifold],
-    ) {
-        for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
-            let manifold = &manifolds_all[*manifold_i];
-            VelocityGroundConstraint::generate(
-                params,
-                *manifold_i,
-                manifold,
-                bodies,
-                &mut self.velocity_constraints,
-                None,
-            );
-        }
-    }
-}
-
-impl SolverConstraints<AnyJointVelocityConstraint> {
-    pub fn init(
-        &mut self,
-        island_id: usize,
-        params: &IntegrationParameters,
-        islands: &IslandManager,
-        bodies: &RigidBodySet,
-        multibody_joints: &MultibodyJointSet,
-        impulse_joints: &[JointGraphEdge],
-        joint_constraint_indices: &[JointIndex],
-    ) {
-        // Generate constraints for impulse_joints.
-        self.not_ground_interactions.clear();
-        self.ground_interactions.clear();
-        self.generic_not_ground_interactions.clear();
-        self.generic_ground_interactions.clear();
-
-        categorize_joints(
-            bodies,
-            multibody_joints,
-            impulse_joints,
-            joint_constraint_indices,
-            &mut self.ground_interactions,
-            &mut self.not_ground_interactions,
-            &mut self.generic_ground_interactions,
-            &mut self.generic_not_ground_interactions,
-        );
-
-        self.velocity_constraints.clear();
-
-        self.interaction_groups.clear_groups();
-        self.interaction_groups.group_joints(
-            island_id,
-            islands,
-            bodies,
-            impulse_joints,
-            &self.not_ground_interactions,
-        );
-
-        self.ground_interaction_groups.clear_groups();
-        self.ground_interaction_groups.group_joints(
-            island_id,
-            islands,
-            bodies,
-            impulse_joints,
-            &self.ground_interactions,
-        );
-        // NOTE: uncomment this do disable SIMD joint resolution.
-        // self.interaction_groups
-        //     .nongrouped_interactions
-        //     .append(&mut self.interaction_groups.grouped_interactions);
-        // self.ground_interaction_groups
-        //     .nongrouped_interactions
-        //     .append(&mut self.ground_interaction_groups.grouped_interactions);
-
-        let mut j_id = 0;
-        self.compute_nongrouped_joint_constraints(
-            params,
-            bodies,
-            multibody_joints,
-            impulse_joints,
-            &mut j_id,
-        );
-        #[cfg(feature = "simd-is-enabled")]
-        {
-            self.compute_grouped_joint_constraints(params, bodies, impulse_joints);
-        }
-        self.compute_generic_joint_constraints(
-            params,
-            bodies,
-            multibody_joints,
-            impulse_joints,
-            &mut j_id,
-        );
-
-        self.compute_nongrouped_joint_ground_constraints(
-            params,
-            bodies,
-            multibody_joints,
-            impulse_joints,
-        );
-        #[cfg(feature = "simd-is-enabled")]
-        {
-            self.compute_grouped_joint_ground_constraints(params, bodies, impulse_joints);
-        }
-        self.compute_generic_ground_joint_constraints(
-            params,
-            bodies,
-            multibody_joints,
-            impulse_joints,
-            &mut j_id,
-        );
-        self.compute_articulation_constraints(
-            params,
-            island_id,
-            islands,
-            multibody_joints,
-            &mut j_id,
-        );
-    }
-
-    fn compute_articulation_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        island_id: usize,
-        islands: &IslandManager,
-        multibody_joints: &MultibodyJointSet,
-        j_id: &mut usize,
-    ) {
-        for handle in islands.active_island(island_id) {
-            if let Some(link) = multibody_joints.rigid_body_link(*handle) {
-                let multibody = multibody_joints.get_multibody(link.multibody).unwrap();
-                if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                    multibody.generate_internal_constraints(
-                        params,
-                        j_id,
-                        &mut self.generic_jacobians,
-                        &mut self.velocity_constraints,
-                        None,
-                    )
-                }
-            }
-        }
-    }
-
-    fn compute_nongrouped_joint_ground_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        multibody_joints: &MultibodyJointSet,
-        joints_all: &[JointGraphEdge],
-    ) {
-        let mut j_id = 0;
-        for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
-            let joint = &joints_all[*joint_i].weight;
-            AnyJointVelocityConstraint::from_joint_ground(
-                params,
-                *joint_i,
-                joint,
-                bodies,
-                multibody_joints,
-                &mut j_id,
-                &mut self.generic_jacobians,
-                &mut self.velocity_constraints,
-                None,
-            );
-        }
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    fn compute_grouped_joint_ground_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        joints_all: &[JointGraphEdge],
-    ) {
-        for joints_i in self
-            .ground_interaction_groups
-            .grouped_interactions
-            .chunks_exact(SIMD_WIDTH)
-        {
-            let joints_id = gather![|ii| joints_i[ii]];
-            let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
-            AnyJointVelocityConstraint::from_wide_joint_ground(
-                params,
-                joints_id,
-                impulse_joints,
-                bodies,
-                &mut self.velocity_constraints,
-                None,
-            );
-        }
-    }
-
-    fn compute_nongrouped_joint_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        multibody_joints: &MultibodyJointSet,
-        joints_all: &[JointGraphEdge],
-        j_id: &mut usize,
-    ) {
-        for joint_i in &self.interaction_groups.nongrouped_interactions {
-            let joint = &joints_all[*joint_i].weight;
-            AnyJointVelocityConstraint::from_joint(
-                params,
-                *joint_i,
-                joint,
-                bodies,
-                multibody_joints,
-                j_id,
-                &mut self.generic_jacobians,
-                &mut self.velocity_constraints,
-                None,
-            );
-        }
-    }
-
-    fn compute_generic_joint_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        multibody_joints: &MultibodyJointSet,
-        joints_all: &[JointGraphEdge],
-        j_id: &mut usize,
-    ) {
-        for joint_i in &self.generic_not_ground_interactions {
-            let joint = &joints_all[*joint_i].weight;
-            AnyJointVelocityConstraint::from_joint(
-                params,
-                *joint_i,
-                joint,
-                bodies,
-                multibody_joints,
-                j_id,
-                &mut self.generic_jacobians,
-                &mut self.velocity_constraints,
-                None,
-            )
-        }
-    }
-
-    fn compute_generic_ground_joint_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        multibody_joints: &MultibodyJointSet,
-        joints_all: &[JointGraphEdge],
-        j_id: &mut usize,
-    ) {
-        for joint_i in &self.generic_ground_interactions {
-            let joint = &joints_all[*joint_i].weight;
-            AnyJointVelocityConstraint::from_joint_ground(
-                params,
-                *joint_i,
-                joint,
-                bodies,
-                multibody_joints,
-                j_id,
-                &mut self.generic_jacobians,
-                &mut self.velocity_constraints,
-                None,
-            )
-        }
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    fn compute_grouped_joint_constraints(
-        &mut self,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        joints_all: &[JointGraphEdge],
-    ) {
-        for joints_i in self
-            .interaction_groups
-            .grouped_interactions
-            .chunks_exact(SIMD_WIDTH)
-        {
-            let joints_id = gather![|ii| joints_i[ii]];
-            let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
-            AnyJointVelocityConstraint::from_wide_joint(
-                params,
-                joints_id,
-                impulse_joints,
-                bodies,
-                &mut self.velocity_constraints,
-                None,
-            );
-        }
-    }
-}

src/dynamics/solver/solver_constraints_set.rs

@@ -0,0 +1,241 @@
+use super::InteractionGroups;
+use crate::math::Real;
+use na::DVector;
+
+pub(crate) trait ConstraintTypes {
+    type OneBody;
+    type TwoBodies;
+    type GenericOneBody;
+    type GenericTwoBodies;
+
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdOneBody;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdTwoBodies;
+
+    type BuilderOneBody;
+    type BuilderTwoBodies;
+
+    type GenericBuilderOneBody;
+    type GenericBuilderTwoBodies;
+
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdBuilderOneBody;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdBuilderTwoBodies;
+}
+
+pub enum AnyConstraintMut<'a, Constraints: ConstraintTypes> {
+    OneBody(&'a mut Constraints::OneBody),
+    TwoBodies(&'a mut Constraints::TwoBodies),
+    GenericOneBody(&'a mut Constraints::GenericOneBody),
+    GenericTwoBodies(&'a mut Constraints::GenericTwoBodies),
+    #[cfg(feature = "simd-is-enabled")]
+    SimdOneBody(&'a mut Constraints::SimdOneBody),
+    #[cfg(feature = "simd-is-enabled")]
+    SimdTwoBodies(&'a mut Constraints::SimdTwoBodies),
+}
+
+pub(crate) struct SolverConstraintsSet<Constraints: ConstraintTypes> {
+    pub generic_jacobians: DVector<Real>,
+    pub two_body_interactions: Vec<usize>,
+    pub one_body_interactions: Vec<usize>,
+    pub generic_two_body_interactions: Vec<usize>,
+    pub generic_one_body_interactions: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+    pub one_body_interaction_groups: InteractionGroups,
+
+    pub velocity_constraints: Vec<Constraints::TwoBodies>,
+    pub generic_velocity_constraints: Vec<Constraints::GenericTwoBodies>,
+    #[cfg(feature = "simd-is-enabled")]
+    pub simd_velocity_constraints: Vec<Constraints::SimdTwoBodies>,
+    pub velocity_one_body_constraints: Vec<Constraints::OneBody>,
+    pub generic_velocity_one_body_constraints: Vec<Constraints::GenericOneBody>,
+    #[cfg(feature = "simd-is-enabled")]
+    pub simd_velocity_one_body_constraints: Vec<Constraints::SimdOneBody>,
+
+    pub velocity_constraints_builder: Vec<Constraints::BuilderTwoBodies>,
+    pub generic_velocity_constraints_builder: Vec<Constraints::GenericBuilderTwoBodies>,
+    #[cfg(feature = "simd-is-enabled")]
+    pub simd_velocity_constraints_builder: Vec<Constraints::SimdBuilderTwoBodies>,
+    pub velocity_one_body_constraints_builder: Vec<Constraints::BuilderOneBody>,
+    pub generic_velocity_one_body_constraints_builder: Vec<Constraints::GenericBuilderOneBody>,
+    #[cfg(feature = "simd-is-enabled")]
+    pub simd_velocity_one_body_constraints_builder: Vec<Constraints::SimdBuilderOneBody>,
+}
+
+impl<Constraints: ConstraintTypes> SolverConstraintsSet<Constraints> {
+    pub fn new() -> Self {
+        Self {
+            generic_jacobians: DVector::zeros(0),
+            two_body_interactions: vec![],
+            one_body_interactions: vec![],
+            generic_two_body_interactions: vec![],
+            generic_one_body_interactions: vec![],
+            interaction_groups: InteractionGroups::new(),
+            one_body_interaction_groups: InteractionGroups::new(),
+
+            velocity_constraints: vec![],
+            generic_velocity_constraints: vec![],
+            #[cfg(feature = "simd-is-enabled")]
+            simd_velocity_constraints: vec![],
+            velocity_one_body_constraints: vec![],
+            generic_velocity_one_body_constraints: vec![],
+            #[cfg(feature = "simd-is-enabled")]
+            simd_velocity_one_body_constraints: vec![],
+
+            velocity_constraints_builder: vec![],
+            generic_velocity_constraints_builder: vec![],
+            #[cfg(feature = "simd-is-enabled")]
+            simd_velocity_constraints_builder: vec![],
+            velocity_one_body_constraints_builder: vec![],
+            generic_velocity_one_body_constraints_builder: vec![],
+            #[cfg(feature = "simd-is-enabled")]
+            simd_velocity_one_body_constraints_builder: vec![],
+        }
+    }
+
+    #[allow(dead_code)] // Useful for debuging.
+    pub fn print_counts(&self) {
+        println!("Solver constraints:");
+        println!(
+            "|__ two_body_interactions: {}",
+            self.two_body_interactions.len()
+        );
+        println!(
+            "|__ one_body_interactions: {}",
+            self.one_body_interactions.len()
+        );
+        println!(
+            "|__ generic_two_body_interactions: {}",
+            self.generic_two_body_interactions.len()
+        );
+        println!(
+            "|__ generic_one_body_interactions: {}",
+            self.generic_one_body_interactions.len()
+        );
+
+        println!(
+            "|__ velocity_constraints: {}",
+            self.velocity_constraints.len()
+        );
+        println!(
+            "|__ generic_velocity_constraints: {}",
+            self.generic_velocity_constraints.len()
+        );
+        #[cfg(feature = "simd-is-enabled")]
+        println!(
+            "|__ simd_velocity_constraints: {}",
+            self.simd_velocity_constraints.len()
+        );
+        println!(
+            "|__ velocity_one_body_constraints: {}",
+            self.velocity_one_body_constraints.len()
+        );
+        println!(
+            "|__ generic_velocity_one_body_constraints: {}",
+            self.generic_velocity_one_body_constraints.len()
+        );
+        #[cfg(feature = "simd-is-enabled")]
+        println!(
+            "|__ simd_velocity_one_body_constraints: {}",
+            self.simd_velocity_one_body_constraints.len()
+        );
+
+        println!(
+            "|__ velocity_constraints_builder: {}",
+            self.velocity_constraints_builder.len()
+        );
+        println!(
+            "|__ generic_velocity_constraints_builder: {}",
+            self.generic_velocity_constraints_builder.len()
+        );
+        #[cfg(feature = "simd-is-enabled")]
+        println!(
+            "|__ simd_velocity_constraints_builder: {}",
+            self.simd_velocity_constraints_builder.len()
+        );
+        println!(
+            "|__ velocity_one_body_constraints_builder: {}",
+            self.velocity_one_body_constraints_builder.len()
+        );
+        println!(
+            "|__ generic_velocity_one_body_constraints_builder: {}",
+            self.generic_velocity_one_body_constraints_builder.len()
+        );
+        #[cfg(feature = "simd-is-enabled")]
+        println!(
+            "|__ simd_velocity_one_body_constraints_builder: {}",
+            self.simd_velocity_one_body_constraints_builder.len()
+        );
+    }
+
+    pub fn clear_constraints(&mut self) {
+        self.generic_jacobians.fill(0.0);
+        self.velocity_constraints.clear();
+        self.velocity_one_body_constraints.clear();
+        self.generic_velocity_constraints.clear();
+        self.generic_velocity_one_body_constraints.clear();
+
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.simd_velocity_constraints.clear();
+            self.simd_velocity_one_body_constraints.clear();
+        }
+    }
+
+    pub fn clear_builders(&mut self) {
+        self.velocity_constraints_builder.clear();
+        self.velocity_one_body_constraints_builder.clear();
+        self.generic_velocity_constraints_builder.clear();
+        self.generic_velocity_one_body_constraints_builder.clear();
+
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.simd_velocity_constraints_builder.clear();
+            self.simd_velocity_one_body_constraints_builder.clear();
+        }
+    }
+
+    // Returns the generic jacobians and a mutable iterator through all the constraints.
+    pub fn iter_constraints_mut(
+        &mut self,
+    ) -> (
+        &DVector<Real>,
+        impl Iterator<Item = AnyConstraintMut<Constraints>>,
+    ) {
+        let jac = &self.generic_jacobians;
+        let a = self
+            .velocity_constraints
+            .iter_mut()
+            .map(AnyConstraintMut::TwoBodies);
+        let b = self
+            .generic_velocity_constraints
+            .iter_mut()
+            .map(AnyConstraintMut::GenericTwoBodies);
+        #[cfg(feature = "simd-is-enabled")]
+        let c = self
+            .simd_velocity_constraints
+            .iter_mut()
+            .map(AnyConstraintMut::SimdTwoBodies);
+        let d = self
+            .velocity_one_body_constraints
+            .iter_mut()
+            .map(AnyConstraintMut::OneBody);
+        let e = self
+            .generic_velocity_one_body_constraints
+            .iter_mut()
+            .map(AnyConstraintMut::GenericOneBody);
+        #[cfg(feature = "simd-is-enabled")]
+        let f = self
+            .simd_velocity_one_body_constraints
+            .iter_mut()
+            .map(AnyConstraintMut::SimdOneBody);
+
+        #[cfg(feature = "simd-is-enabled")]
+        return (jac, a.chain(b).chain(c).chain(d).chain(e).chain(f));
+
+        #[cfg(not(feature = "simd-is-enabled"))]
+        return (jac, a.chain(b).chain(d).chain(e));
+    }
+}

src/dynamics/solver/solver_vel.rs

@@ -1,17 +1,19 @@
 use crate::math::{AngVector, Vector, SPATIAL_DIM};
-use crate::utils::WReal;
+use crate::utils::SimdRealCopy;
 use na::{DVectorView, DVectorViewMut, Scalar};
 use std::ops::{AddAssign, Sub};
 
 #[derive(Copy, Clone, Debug, Default)]
 #[repr(C)]
 //#[repr(align(64))]
-pub struct DeltaVel<N: Scalar + Copy> {
+pub struct SolverVel<N: Scalar + Copy> {
+    // The linear velocity of a solver body.
     pub linear: Vector<N>,
+    // The angular velocity, multiplied by the inverse sqrt angular inertia, of a solver body.
     pub angular: AngVector<N>,
 }
 
-impl<N: Scalar + Copy> DeltaVel<N> {
+impl<N: Scalar + Copy> SolverVel<N> {
     pub fn as_slice(&self) -> &[N; SPATIAL_DIM] {
         unsafe { std::mem::transmute(self) }
     }
@@ -29,7 +31,7 @@ impl<N: Scalar + Copy> DeltaVel<N> {
     }
 }
 
-impl<N: WReal> DeltaVel<N> {
+impl<N: SimdRealCopy> SolverVel<N> {
     pub fn zero() -> Self {
         Self {
             linear: na::zero(),
@@ -38,18 +40,18 @@ impl<N: WReal> DeltaVel<N> {
     }
 }
 
-impl<N: WReal> AddAssign for DeltaVel<N> {
+impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
     fn add_assign(&mut self, rhs: Self) {
         self.linear += rhs.linear;
         self.angular += rhs.angular;
     }
 }
 
-impl<N: WReal> Sub for DeltaVel<N> {
+impl<N: SimdRealCopy> Sub for SolverVel<N> {
     type Output = Self;
 
     fn sub(self, rhs: Self) -> Self {
-        DeltaVel {
+        SolverVel {
             linear: self.linear - rhs.linear,
             angular: self.angular - rhs.angular,
         }

src/dynamics/solver/velocity_constraint.rs

@@ -1,441 +0,0 @@
-use crate::dynamics::solver::{
-    GenericVelocityConstraint, GenericVelocityGroundConstraint, VelocityGroundConstraint,
-};
-#[cfg(feature = "simd-is-enabled")]
-use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
-use crate::geometry::{ContactManifold, ContactManifoldIndex};
-use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
-use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot};
-use na::DVector;
-
-use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
-
-//#[repr(align(64))]
-#[derive(Copy, Clone, Debug)]
-pub(crate) enum AnyVelocityConstraint {
-    NongroupedGround(VelocityGroundConstraint),
-    Nongrouped(VelocityConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    GroupedGround(WVelocityGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    Grouped(WVelocityConstraint),
-    NongroupedGenericGround(GenericVelocityGroundConstraint),
-    NongroupedGeneric(GenericVelocityConstraint),
-    #[allow(dead_code)] // The Empty variant is only used with parallel code.
-    Empty,
-}
-
-impl AnyVelocityConstraint {
-    #[cfg(target_arch = "wasm32")]
-    pub fn as_nongrouped_mut(&mut self) -> Option<&mut VelocityConstraint> {
-        if let AnyVelocityConstraint::Nongrouped(c) = self {
-            Some(c)
-        } else {
-            None
-        }
-    }
-
-    #[cfg(target_arch = "wasm32")]
-    pub fn as_nongrouped_ground_mut(&mut self) -> Option<&mut VelocityGroundConstraint> {
-        if let AnyVelocityConstraint::NongroupedGround(c) = self {
-            Some(c)
-        } else {
-            None
-        }
-    }
-
-    pub fn remove_bias_from_rhs(&mut self) {
-        match self {
-            AnyVelocityConstraint::Nongrouped(c) => c.remove_cfm_and_bias_from_rhs(),
-            AnyVelocityConstraint::NongroupedGround(c) => c.remove_cfm_and_bias_from_rhs(),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::Grouped(c) => c.remove_cfm_and_bias_from_rhs(),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::GroupedGround(c) => c.remove_cfm_and_bias_from_rhs(),
-            AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_cfm_and_bias_from_rhs(),
-            AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_cfm_and_bias_from_rhs(),
-            AnyVelocityConstraint::Empty => unreachable!(),
-        }
-    }
-
-    pub fn solve(
-        &mut self,
-        jacobians: &DVector<Real>,
-        mj_lambdas: &mut [DeltaVel<Real>],
-        generic_mj_lambdas: &mut DVector<Real>,
-        solve_restitution: bool,
-        solve_friction: bool,
-    ) {
-        match self {
-            AnyVelocityConstraint::NongroupedGround(c) => {
-                c.solve(mj_lambdas, solve_restitution, solve_friction)
-            }
-            AnyVelocityConstraint::Nongrouped(c) => {
-                c.solve(mj_lambdas, solve_restitution, solve_friction)
-            }
-            #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::GroupedGround(c) => {
-                c.solve(mj_lambdas, solve_restitution, solve_friction)
-            }
-            #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::Grouped(c) => {
-                c.solve(mj_lambdas, solve_restitution, solve_friction)
-            }
-            AnyVelocityConstraint::NongroupedGeneric(c) => c.solve(
-                jacobians,
-                mj_lambdas,
-                generic_mj_lambdas,
-                solve_restitution,
-                solve_friction,
-            ),
-            AnyVelocityConstraint::NongroupedGenericGround(c) => c.solve(
-                jacobians,
-                generic_mj_lambdas,
-                solve_restitution,
-                solve_friction,
-            ),
-            AnyVelocityConstraint::Empty => unreachable!(),
-        }
-    }
-
-    pub fn writeback_impulses(&self, manifold_all: &mut [&mut ContactManifold]) {
-        match self {
-            AnyVelocityConstraint::NongroupedGround(c) => c.writeback_impulses(manifold_all),
-            AnyVelocityConstraint::Nongrouped(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::Grouped(c) => c.writeback_impulses(manifold_all),
-            AnyVelocityConstraint::NongroupedGeneric(c) => c.writeback_impulses(manifold_all),
-            AnyVelocityConstraint::NongroupedGenericGround(c) => c.writeback_impulses(manifold_all),
-            AnyVelocityConstraint::Empty => unreachable!(),
-        }
-    }
-}
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityConstraint {
-    pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
-    #[cfg(feature = "dim3")]
-    pub tangent1: Vector<Real>, // One of the friction force directions.
-    pub im1: Vector<Real>,
-    pub im2: Vector<Real>,
-    pub cfm_factor: Real,
-    pub limit: Real,
-    pub mj_lambda1: usize,
-    pub mj_lambda2: usize,
-    pub manifold_id: ContactManifoldIndex,
-    pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
-    pub num_contacts: u8,
-    pub elements: [VelocityConstraintElement<Real>; MAX_MANIFOLD_POINTS],
-}
-
-impl VelocityConstraint {
-    #[cfg(feature = "parallel")]
-    pub fn num_active_constraints_and_jacobian_lines(manifold: &ContactManifold) -> (usize, usize) {
-        let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
-        (
-            manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize,
-            manifold.data.solver_contacts.len() * DIM,
-        )
-    }
-
-    pub fn generate(
-        params: &IntegrationParameters,
-        manifold_id: ContactManifoldIndex,
-        manifold: &ContactManifold,
-        bodies: &RigidBodySet,
-        out_constraints: &mut Vec<AnyVelocityConstraint>,
-        insert_at: Option<usize>,
-    ) {
-        assert_eq!(manifold.data.relative_dominance, 0);
-
-        let cfm_factor = params.cfm_factor();
-        let inv_dt = params.inv_dt();
-        let erp_inv_dt = params.erp_inv_dt();
-
-        let handle1 = manifold.data.rigid_body1.unwrap();
-        let handle2 = manifold.data.rigid_body2.unwrap();
-
-        let rb1 = &bodies[handle1];
-        let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
-        let rb2 = &bodies[handle2];
-        let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
-        let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
-
-        let mj_lambda1 = rb1.ids.active_set_offset;
-        let mj_lambda2 = rb2.ids.active_set_offset;
-        let force_dir1 = -manifold.data.normal;
-
-        #[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);
-
-        for (_l, manifold_points) in manifold
-            .data
-            .solver_contacts
-            .chunks(MAX_MANIFOLD_POINTS)
-            .enumerate()
-        {
-            let mut is_fast_contact = false;
-
-            #[cfg(not(target_arch = "wasm32"))]
-            let mut constraint = VelocityConstraint {
-                dir1: force_dir1,
-                #[cfg(feature = "dim3")]
-                tangent1: tangents1[0],
-                elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
-                im1: mprops1.effective_inv_mass,
-                im2: mprops2.effective_inv_mass,
-                cfm_factor,
-                limit: 0.0,
-                mj_lambda1,
-                mj_lambda2,
-                manifold_id,
-                manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
-                num_contacts: manifold_points.len() as u8,
-            };
-
-            // TODO: this is a WIP optimization for WASM platforms.
-            // For some reasons, the compiler does not inline the `Vec::push` method
-            // in this method. This generates two memset and one memcpy which are both very
-            // expansive.
-            // This would likely be solved by some kind of "placement-push" (like emplace in C++).
-            // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
-            // avoid spurious copying.
-            // Is this optimization beneficial when targeting non-WASM platforms?
-            //
-            // NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way
-            // for the moment.
-            #[cfg(target_arch = "wasm32")]
-            let constraint = if insert_at.is_none() {
-                let new_len = out_constraints.len() + 1;
-                unsafe {
-                    #[allow(invalid_value)]
-                    out_constraints.resize_with(new_len, || {
-                        AnyVelocityConstraint::Nongrouped(
-                            std::mem::MaybeUninit::uninit().assume_init(),
-                        )
-                    });
-                }
-                out_constraints
-                    .last_mut()
-                    .unwrap()
-                    .as_nongrouped_mut()
-                    .unwrap()
-            } else {
-                unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
-            };
-
-            #[cfg(target_arch = "wasm32")]
-            {
-                constraint.dir1 = force_dir1;
-                #[cfg(feature = "dim3")]
-                {
-                    constraint.tangent1 = tangents1[0];
-                }
-                constraint.im1 = mprops1.effective_inv_mass;
-                constraint.im2 = mprops2.effective_inv_mass;
-                constraint.cfm_factor = cfm_factor;
-                constraint.limit = 0.0;
-                constraint.mj_lambda1 = mj_lambda1;
-                constraint.mj_lambda2 = mj_lambda2;
-                constraint.manifold_id = manifold_id;
-                constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
-                constraint.num_contacts = manifold_points.len() as u8;
-            }
-
-            for k in 0..manifold_points.len() {
-                let manifold_point = &manifold_points[k];
-                let dp1 = manifold_point.point - mprops1.world_com;
-                let dp2 = manifold_point.point - mprops2.world_com;
-
-                let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
-                let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
-
-                constraint.limit = manifold_point.friction;
-                constraint.manifold_contact_id[k] = manifold_point.contact_id;
-
-                // Normal part.
-                {
-                    let gcross1 = mprops1
-                        .effective_world_inv_inertia_sqrt
-                        .transform_vector(dp1.gcross(force_dir1));
-                    let gcross2 = mprops2
-                        .effective_world_inv_inertia_sqrt
-                        .transform_vector(dp2.gcross(-force_dir1));
-
-                    let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
-                    let projected_mass = utils::inv(
-                        force_dir1.dot(&imsum.component_mul(&force_dir1))
-                            + gcross1.gdot(gcross1)
-                            + gcross2.gdot(gcross2),
-                    );
-
-                    let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
-                    let is_resting = 1.0 - is_bouncy;
-
-                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
-                        * (vel1 - vel2).dot(&force_dir1);
-                    rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting;
-                    let rhs_bias = /* is_resting
-                        * */  erp_inv_dt
-                        * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0);
-
-                    let rhs = rhs_wo_bias + rhs_bias;
-                    is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5);
-
-                    constraint.elements[k].normal_part = VelocityConstraintNormalPart {
-                        gcross1,
-                        gcross2,
-                        rhs,
-                        rhs_wo_bias,
-                        impulse: na::zero(),
-                        r: projected_mass,
-                    };
-                }
-
-                // Tangent parts.
-                {
-                    constraint.elements[k].tangent_part.impulse = na::zero();
-
-                    for j in 0..DIM - 1 {
-                        let gcross1 = mprops1
-                            .effective_world_inv_inertia_sqrt
-                            .transform_vector(dp1.gcross(tangents1[j]));
-                        let gcross2 = mprops2
-                            .effective_world_inv_inertia_sqrt
-                            .transform_vector(dp2.gcross(-tangents1[j]));
-                        let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
-                        let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
-                            + gcross1.gdot(gcross1)
-                            + gcross2.gdot(gcross2);
-                        let rhs =
-                            (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
-
-                        constraint.elements[k].tangent_part.gcross1[j] = gcross1;
-                        constraint.elements[k].tangent_part.gcross2[j] = gcross2;
-                        constraint.elements[k].tangent_part.rhs[j] = rhs;
-                        constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
-                            utils::inv(r)
-                        } else {
-                            r
-                        };
-                    }
-
-                    #[cfg(feature = "dim3")]
-                    {
-                        constraint.elements[k].tangent_part.r[2] = 2.0
-                            * (constraint.elements[k].tangent_part.gcross1[0]
-                                .gdot(constraint.elements[k].tangent_part.gcross1[1])
-                                + constraint.elements[k].tangent_part.gcross2[0]
-                                    .gdot(constraint.elements[k].tangent_part.gcross2[1]));
-                    }
-                }
-            }
-
-            constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
-
-            #[cfg(not(target_arch = "wasm32"))]
-            if let Some(at) = insert_at {
-                out_constraints[at + _l] = AnyVelocityConstraint::Nongrouped(constraint);
-            } else {
-                out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
-            }
-        }
-    }
-
-    pub fn solve(
-        &mut self,
-        mj_lambdas: &mut [DeltaVel<Real>],
-        solve_normal: bool,
-        solve_friction: bool,
-    ) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        VelocityConstraintElement::solve_group(
-            self.cfm_factor,
-            &mut self.elements[..self.num_contacts as usize],
-            &self.dir1,
-            #[cfg(feature = "dim3")]
-            &self.tangent1,
-            &self.im1,
-            &self.im2,
-            self.limit,
-            &mut mj_lambda1,
-            &mut mj_lambda2,
-            solve_normal,
-            solve_friction,
-        );
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
-        let manifold = &mut manifolds_all[self.manifold_id];
-
-        for k in 0..self.num_contacts as usize {
-            let contact_id = self.manifold_contact_id[k];
-            let active_contact = &mut manifold.points[contact_id as usize];
-            active_contact.data.impulse = self.elements[k].normal_part.impulse;
-
-            #[cfg(feature = "dim2")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
-            }
-            #[cfg(feature = "dim3")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
-            }
-        }
-    }
-
-    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
-        self.cfm_factor = 1.0;
-        for elt in &mut self.elements {
-            elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
-        }
-    }
-}
-
-#[inline(always)]
-#[cfg(feature = "dim3")]
-pub(crate) fn compute_tangent_contact_directions<N>(
-    force_dir1: &Vector<N>,
-    linvel1: &Vector<N>,
-    linvel2: &Vector<N>,
-) -> [Vector<N>; DIM - 1]
-where
-    N: utils::WReal,
-    Vector<N>: WBasis,
-{
-    use na::SimdValue;
-
-    // Compute the tangent direction. Pick the direction of
-    // the linear relative velocity, if it is not too small.
-    // Otherwise use a fallback direction.
-    let relative_linvel = linvel1 - linvel2;
-    let mut tangent_relative_linvel =
-        relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel));
-
-    let tangent_linvel_norm = {
-        let _disable_fe_except =
-            crate::utils::DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions();
-        tangent_relative_linvel.normalize_mut()
-    };
-
-    const THRESHOLD: Real = 1.0e-4;
-    let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD));
-    let tangent_fallback = force_dir1.orthonormal_vector();
-
-    let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel);
-    let bitangent1 = force_dir1.cross(&tangent1);
-
-    [tangent1, bitangent1]
-}

src/dynamics/solver/velocity_constraint_element.rs

@@ -1,211 +0,0 @@
-use super::DeltaVel;
-use crate::math::{AngVector, Vector, DIM};
-use crate::utils::{WBasis, WDot, WReal};
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityConstraintTangentPart<N: WReal> {
-    pub gcross1: [AngVector<N>; DIM - 1],
-    pub gcross2: [AngVector<N>; DIM - 1],
-    pub rhs: [N; DIM - 1],
-    #[cfg(feature = "dim2")]
-    pub impulse: na::Vector1<N>,
-    #[cfg(feature = "dim3")]
-    pub impulse: na::Vector2<N>,
-    #[cfg(feature = "dim2")]
-    pub r: [N; 1],
-    #[cfg(feature = "dim3")]
-    pub r: [N; DIM],
-}
-
-impl<N: WReal> VelocityConstraintTangentPart<N> {
-    fn zero() -> Self {
-        Self {
-            gcross1: [na::zero(); DIM - 1],
-            gcross2: [na::zero(); DIM - 1],
-            rhs: [na::zero(); DIM - 1],
-            impulse: na::zero(),
-            #[cfg(feature = "dim2")]
-            r: [na::zero(); 1],
-            #[cfg(feature = "dim3")]
-            r: [na::zero(); DIM],
-        }
-    }
-
-    #[inline]
-    pub fn solve(
-        &mut self,
-        tangents1: [&Vector<N>; DIM - 1],
-        im1: &Vector<N>,
-        im2: &Vector<N>,
-        limit: N,
-        mj_lambda1: &mut DeltaVel<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-    {
-        #[cfg(feature = "dim2")]
-        {
-            let dvel = tangents1[0].dot(&mj_lambda1.linear)
-                + self.gcross1[0].gdot(mj_lambda1.angular)
-                - tangents1[0].dot(&mj_lambda2.linear)
-                + self.gcross2[0].gdot(mj_lambda2.angular)
-                + self.rhs[0];
-            let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
-            let dlambda = new_impulse - self.impulse[0];
-            self.impulse[0] = new_impulse;
-
-            mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda;
-            mj_lambda1.angular += self.gcross1[0] * dlambda;
-
-            mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda;
-            mj_lambda2.angular += self.gcross2[0] * dlambda;
-        }
-
-        #[cfg(feature = "dim3")]
-        {
-            let dvel_0 = tangents1[0].dot(&mj_lambda1.linear)
-                + self.gcross1[0].gdot(mj_lambda1.angular)
-                - tangents1[0].dot(&mj_lambda2.linear)
-                + self.gcross2[0].gdot(mj_lambda2.angular)
-                + self.rhs[0];
-            let dvel_1 = tangents1[1].dot(&mj_lambda1.linear)
-                + self.gcross1[1].gdot(mj_lambda1.angular)
-                - tangents1[1].dot(&mj_lambda2.linear)
-                + self.gcross2[1].gdot(mj_lambda2.angular)
-                + self.rhs[1];
-
-            let dvel_00 = dvel_0 * dvel_0;
-            let dvel_11 = dvel_1 * dvel_1;
-            let dvel_01 = dvel_0 * dvel_1;
-            let inv_lhs = (dvel_00 + dvel_11)
-                * crate::utils::simd_inv(
-                    dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
-                );
-            let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
-            let new_impulse = self.impulse - delta_impulse;
-            let new_impulse = {
-                let _disable_fe_except =
-                        crate::utils::DisableFloatingPointExceptionsFlags::
-                        disable_floating_point_exceptions();
-                new_impulse.simd_cap_magnitude(limit)
-            };
-
-            let dlambda = new_impulse - self.impulse;
-            self.impulse = new_impulse;
-
-            mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda[0]
-                + tangents1[1].component_mul(im1) * dlambda[1];
-            mj_lambda1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
-
-            mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
-                + tangents1[1].component_mul(im2) * -dlambda[1];
-            mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
-        }
-    }
-}
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityConstraintNormalPart<N: WReal> {
-    pub gcross1: AngVector<N>,
-    pub gcross2: AngVector<N>,
-    pub rhs: N,
-    pub rhs_wo_bias: N,
-    pub impulse: N,
-    pub r: N,
-}
-
-impl<N: WReal> VelocityConstraintNormalPart<N> {
-    fn zero() -> Self {
-        Self {
-            gcross1: na::zero(),
-            gcross2: na::zero(),
-            rhs: na::zero(),
-            rhs_wo_bias: na::zero(),
-            impulse: na::zero(),
-            r: na::zero(),
-        }
-    }
-
-    #[inline]
-    pub fn solve(
-        &mut self,
-        cfm_factor: N,
-        dir1: &Vector<N>,
-        im1: &Vector<N>,
-        im2: &Vector<N>,
-        mj_lambda1: &mut DeltaVel<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-    {
-        let dvel = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular)
-            - dir1.dot(&mj_lambda2.linear)
-            + self.gcross2.gdot(mj_lambda2.angular)
-            + self.rhs;
-        let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
-        let dlambda = new_impulse - self.impulse;
-        self.impulse = new_impulse;
-
-        mj_lambda1.linear += dir1.component_mul(im1) * dlambda;
-        mj_lambda1.angular += self.gcross1 * dlambda;
-
-        mj_lambda2.linear += dir1.component_mul(im2) * -dlambda;
-        mj_lambda2.angular += self.gcross2 * dlambda;
-    }
-}
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityConstraintElement<N: WReal> {
-    pub normal_part: VelocityConstraintNormalPart<N>,
-    pub tangent_part: VelocityConstraintTangentPart<N>,
-}
-
-impl<N: WReal> VelocityConstraintElement<N> {
-    pub fn zero() -> Self {
-        Self {
-            normal_part: VelocityConstraintNormalPart::zero(),
-            tangent_part: VelocityConstraintTangentPart::zero(),
-        }
-    }
-
-    #[inline]
-    pub fn solve_group(
-        cfm_factor: N,
-        elements: &mut [Self],
-        dir1: &Vector<N>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im1: &Vector<N>,
-        im2: &Vector<N>,
-        limit: N,
-        mj_lambda1: &mut DeltaVel<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-        solve_normal: bool,
-        solve_friction: bool,
-    ) where
-        Vector<N>: WBasis,
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-    {
-        // Solve penetration.
-        if solve_normal {
-            for element in elements.iter_mut() {
-                element
-                    .normal_part
-                    .solve(cfm_factor, &dir1, im1, im2, mj_lambda1, mj_lambda2);
-            }
-        }
-
-        // Solve friction.
-        if solve_friction {
-            #[cfg(feature = "dim3")]
-            let tangents1 = [tangent1, &dir1.cross(&tangent1)];
-            #[cfg(feature = "dim2")]
-            let tangents1 = [&dir1.orthonormal_vector()];
-
-            for element in elements.iter_mut() {
-                let limit = limit * element.normal_part.impulse;
-                let part = &mut element.tangent_part;
-                part.solve(tangents1, im1, im2, limit, mj_lambda1, mj_lambda2);
-            }
-        }
-    }
-}

src/dynamics/solver/velocity_ground_constraint.rs

@@ -1,281 +0,0 @@
-use super::{
-    AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
-    VelocityGroundConstraintNormalPart,
-};
-use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
-#[cfg(feature = "dim2")]
-use crate::utils::WBasis;
-use crate::utils::{self, WAngularInertia, WCross, WDot};
-
-use crate::dynamics::{IntegrationParameters, RigidBodySet, RigidBodyVelocity};
-use crate::geometry::{ContactManifold, ContactManifoldIndex};
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityGroundConstraint {
-    pub mj_lambda2: usize,
-    pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
-    #[cfg(feature = "dim3")]
-    pub tangent1: Vector<Real>, // One of the friction force directions.
-    pub im2: Vector<Real>,
-    pub cfm_factor: Real,
-    pub limit: Real,
-    pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS],
-
-    pub manifold_id: ContactManifoldIndex,
-    pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
-    pub num_contacts: u8,
-}
-
-impl VelocityGroundConstraint {
-    pub fn generate(
-        params: &IntegrationParameters,
-        manifold_id: ContactManifoldIndex,
-        manifold: &ContactManifold,
-        bodies: &RigidBodySet,
-        out_constraints: &mut Vec<AnyVelocityConstraint>,
-        insert_at: Option<usize>,
-    ) {
-        let cfm_factor = params.cfm_factor();
-        let inv_dt = params.inv_dt();
-        let erp_inv_dt = params.erp_inv_dt();
-
-        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 rb2 = &bodies[handle2.unwrap()];
-        let vels2 = &rb2.vels;
-        let mprops2 = &rb2.mprops;
-
-        #[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 mj_lambda2 = rb2.ids.active_set_offset;
-
-        for (_l, manifold_points) in manifold
-            .data
-            .solver_contacts
-            .chunks(MAX_MANIFOLD_POINTS)
-            .enumerate()
-        {
-            #[cfg(not(target_arch = "wasm32"))]
-            let mut constraint = VelocityGroundConstraint {
-                dir1: force_dir1,
-                #[cfg(feature = "dim3")]
-                tangent1: tangents1[0],
-                elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
-                im2: mprops2.effective_inv_mass,
-                cfm_factor,
-                limit: 0.0,
-                mj_lambda2,
-                manifold_id,
-                manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
-                num_contacts: manifold_points.len() as u8,
-            };
-
-            // TODO: this is a WIP optimization for WASM platforms.
-            // For some reasons, the compiler does not inline the `Vec::push` method
-            // in this method. This generates two memset and one memcpy which are both very
-            // expansive.
-            // This would likely be solved by some kind of "placement-push" (like emplace in C++).
-            // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
-            // avoid spurious copying.
-            // Is this optimization beneficial when targeting non-WASM platforms?
-            //
-            // NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way
-            // for the moment.
-            #[cfg(target_arch = "wasm32")]
-            let constraint = if insert_at.is_none() {
-                let new_len = out_constraints.len() + 1;
-                unsafe {
-                    #[allow(invalid_value)]
-                    out_constraints.resize_with(new_len, || {
-                        AnyVelocityConstraint::NongroupedGround(
-                            std::mem::MaybeUninit::uninit().assume_init(),
-                        )
-                    });
-                }
-                out_constraints
-                    .last_mut()
-                    .unwrap()
-                    .as_nongrouped_ground_mut()
-                    .unwrap()
-            } else {
-                unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
-            };
-
-            #[cfg(target_arch = "wasm32")]
-            {
-                constraint.dir1 = force_dir1;
-                #[cfg(feature = "dim3")]
-                {
-                    constraint.tangent1 = tangents1[0];
-                }
-                constraint.im2 = mprops2.effective_inv_mass;
-                constraint.cfm_factor = cfm_factor;
-                constraint.limit = 0.0;
-                constraint.mj_lambda2 = mj_lambda2;
-                constraint.manifold_id = manifold_id;
-                constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
-                constraint.num_contacts = manifold_points.len() as u8;
-            }
-
-            let mut is_fast_contact = false;
-
-            for k in 0..manifold_points.len() {
-                let manifold_point = &manifold_points[k];
-                let dp2 = manifold_point.point - mprops2.world_com;
-                let dp1 = manifold_point.point - world_com1;
-                let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
-                let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
-
-                constraint.limit = manifold_point.friction;
-                constraint.manifold_contact_id[k] = manifold_point.contact_id;
-
-                // Normal part.
-                {
-                    let gcross2 = mprops2
-                        .effective_world_inv_inertia_sqrt
-                        .transform_vector(dp2.gcross(-force_dir1));
-
-                    let projected_mass = utils::inv(
-                        force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
-                            + gcross2.gdot(gcross2),
-                    );
-
-                    let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
-                    let is_resting = 1.0 - is_bouncy;
-
-                    let dvel = (vel1 - vel2).dot(&force_dir1);
-                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel;
-                    rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting;
-                    let rhs_bias = /* is_resting
-                        * */ erp_inv_dt
-                        * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0);
-
-                    let rhs = rhs_wo_bias + rhs_bias;
-                    is_fast_contact =
-                        is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5);
-
-                    constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
-                        gcross2,
-                        rhs,
-                        rhs_wo_bias,
-                        impulse: na::zero(),
-                        r: projected_mass,
-                    };
-                }
-
-                // Tangent parts.
-                {
-                    constraint.elements[k].tangent_part.impulse = na::zero();
-
-                    for j in 0..DIM - 1 {
-                        let gcross2 = mprops2
-                            .effective_world_inv_inertia_sqrt
-                            .transform_vector(dp2.gcross(-tangents1[j]));
-                        let r = tangents1[j]
-                            .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
-                            + gcross2.gdot(gcross2);
-                        let rhs = (vel1 - vel2
-                            + flipped_multiplier * manifold_point.tangent_velocity)
-                            .dot(&tangents1[j]);
-
-                        constraint.elements[k].tangent_part.gcross2[j] = gcross2;
-                        constraint.elements[k].tangent_part.rhs[j] = rhs;
-                        constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
-                            utils::inv(r)
-                        } else {
-                            r
-                        };
-                    }
-
-                    #[cfg(feature = "dim3")]
-                    {
-                        constraint.elements[k].tangent_part.r[2] = 2.0
-                            * constraint.elements[k].tangent_part.gcross2[0]
-                                .gdot(constraint.elements[k].tangent_part.gcross2[1]);
-                    }
-                }
-            }
-
-            constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
-
-            #[cfg(not(target_arch = "wasm32"))]
-            if let Some(at) = insert_at {
-                out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint);
-            } else {
-                out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
-            }
-        }
-    }
-
-    pub fn solve(
-        &mut self,
-        mj_lambdas: &mut [DeltaVel<Real>],
-        solve_normal: bool,
-        solve_friction: bool,
-    ) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        VelocityGroundConstraintElement::solve_group(
-            self.cfm_factor,
-            &mut self.elements[..self.num_contacts as usize],
-            &self.dir1,
-            #[cfg(feature = "dim3")]
-            &self.tangent1,
-            &self.im2,
-            self.limit,
-            &mut mj_lambda2,
-            solve_normal,
-            solve_friction,
-        );
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
-    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
-        let manifold = &mut manifolds_all[self.manifold_id];
-
-        for k in 0..self.num_contacts as usize {
-            let contact_id = self.manifold_contact_id[k];
-            let active_contact = &mut manifold.points[contact_id as usize];
-            active_contact.data.impulse = self.elements[k].normal_part.impulse;
-
-            #[cfg(feature = "dim2")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
-            }
-            #[cfg(feature = "dim3")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
-            }
-        }
-    }
-
-    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
-        self.cfm_factor = 1.0;
-        for elt in &mut self.elements {
-            elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
-        }
-    }
-}

src/dynamics/solver/velocity_ground_constraint_element.rs

@@ -1,181 +0,0 @@
-use super::DeltaVel;
-use crate::math::{AngVector, Vector, DIM};
-use crate::utils::{WBasis, WDot, WReal};
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityGroundConstraintTangentPart<N: WReal> {
-    pub gcross2: [AngVector<N>; DIM - 1],
-    pub rhs: [N; DIM - 1],
-    #[cfg(feature = "dim2")]
-    pub impulse: na::Vector1<N>,
-    #[cfg(feature = "dim3")]
-    pub impulse: na::Vector2<N>,
-    #[cfg(feature = "dim2")]
-    pub r: [N; 1],
-    #[cfg(feature = "dim3")]
-    pub r: [N; DIM],
-}
-
-impl<N: WReal> VelocityGroundConstraintTangentPart<N> {
-    fn zero() -> Self {
-        Self {
-            gcross2: [na::zero(); DIM - 1],
-            rhs: [na::zero(); DIM - 1],
-            impulse: na::zero(),
-            #[cfg(feature = "dim2")]
-            r: [na::zero(); 1],
-            #[cfg(feature = "dim3")]
-            r: [na::zero(); DIM],
-        }
-    }
-
-    #[inline]
-    pub fn solve(
-        &mut self,
-        tangents1: [&Vector<N>; DIM - 1],
-        im2: &Vector<N>,
-        limit: N,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-    {
-        #[cfg(feature = "dim2")]
-        {
-            let dvel = -tangents1[0].dot(&mj_lambda2.linear)
-                + self.gcross2[0].gdot(mj_lambda2.angular)
-                + self.rhs[0];
-            let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
-            let dlambda = new_impulse - self.impulse[0];
-            self.impulse[0] = new_impulse;
-
-            mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda;
-            mj_lambda2.angular += self.gcross2[0] * dlambda;
-        }
-
-        #[cfg(feature = "dim3")]
-        {
-            let dvel_0 = -tangents1[0].dot(&mj_lambda2.linear)
-                + self.gcross2[0].gdot(mj_lambda2.angular)
-                + self.rhs[0];
-            let dvel_1 = -tangents1[1].dot(&mj_lambda2.linear)
-                + self.gcross2[1].gdot(mj_lambda2.angular)
-                + self.rhs[1];
-
-            let dvel_00 = dvel_0 * dvel_0;
-            let dvel_11 = dvel_1 * dvel_1;
-            let dvel_01 = dvel_0 * dvel_1;
-            let inv_lhs = (dvel_00 + dvel_11)
-                * crate::utils::simd_inv(
-                    dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
-                );
-            let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
-            let new_impulse = self.impulse - delta_impulse;
-            let new_impulse = {
-                let _disable_fe_except =
-                    crate::utils::DisableFloatingPointExceptionsFlags::
-                    disable_floating_point_exceptions();
-                new_impulse.simd_cap_magnitude(limit)
-            };
-            let dlambda = new_impulse - self.impulse;
-            self.impulse = new_impulse;
-
-            mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
-                + tangents1[1].component_mul(im2) * -dlambda[1];
-            mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
-        }
-    }
-}
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityGroundConstraintNormalPart<N: WReal> {
-    pub gcross2: AngVector<N>,
-    pub rhs: N,
-    pub rhs_wo_bias: N,
-    pub impulse: N,
-    pub r: N,
-}
-
-impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
-    fn zero() -> Self {
-        Self {
-            gcross2: na::zero(),
-            rhs: na::zero(),
-            rhs_wo_bias: na::zero(),
-            impulse: na::zero(),
-            r: na::zero(),
-        }
-    }
-
-    #[inline]
-    pub fn solve(
-        &mut self,
-        cfm_factor: N,
-        dir1: &Vector<N>,
-        im2: &Vector<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-    {
-        let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
-        let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
-        let dlambda = new_impulse - self.impulse;
-        self.impulse = new_impulse;
-
-        mj_lambda2.linear += dir1.component_mul(im2) * -dlambda;
-        mj_lambda2.angular += self.gcross2 * dlambda;
-    }
-}
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityGroundConstraintElement<N: WReal> {
-    pub normal_part: VelocityGroundConstraintNormalPart<N>,
-    pub tangent_part: VelocityGroundConstraintTangentPart<N>,
-}
-
-impl<N: WReal> VelocityGroundConstraintElement<N> {
-    pub fn zero() -> Self {
-        Self {
-            normal_part: VelocityGroundConstraintNormalPart::zero(),
-            tangent_part: VelocityGroundConstraintTangentPart::zero(),
-        }
-    }
-
-    #[inline]
-    pub fn solve_group(
-        cfm_factor: N,
-        elements: &mut [Self],
-        dir1: &Vector<N>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im2: &Vector<N>,
-        limit: N,
-        mj_lambda2: &mut DeltaVel<N>,
-        solve_normal: bool,
-        solve_friction: bool,
-    ) where
-        Vector<N>: WBasis,
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-    {
-        // Solve penetration.
-        if solve_normal {
-            for element in elements.iter_mut() {
-                element
-                    .normal_part
-                    .solve(cfm_factor, &dir1, im2, mj_lambda2);
-            }
-        }
-
-        // Solve friction.
-        if solve_friction {
-            #[cfg(feature = "dim3")]
-            let tangents1 = [tangent1, &dir1.cross(&tangent1)];
-            #[cfg(feature = "dim2")]
-            let tangents1 = [&dir1.orthonormal_vector()];
-
-            for element in elements.iter_mut() {
-                let limit = limit * element.normal_part.impulse;
-                let part = &mut element.tangent_part;
-                part.solve(tangents1, im2, limit, mj_lambda2);
-            }
-        }
-    }
-}

src/dynamics/solver/velocity_solver.rs

@@ -1,48 +1,99 @@
-use super::AnyJointVelocityConstraint;
+use super::{JointConstraintTypes, SolverConstraintsSet};
+use crate::dynamics::solver::solver_body::SolverBody;
 use crate::dynamics::{
-    solver::{AnyVelocityConstraint, DeltaVel},
-    IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet,
+    solver::{ContactConstraintTypes, SolverVel},
+    IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
+    MultibodyLinkId, RigidBodySet,
 };
-use crate::geometry::ContactManifold;
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::Real;
-use crate::utils::WAngularInertia;
+use crate::prelude::RigidBodyVelocity;
+use crate::utils::SimdAngularInertia;
 use na::DVector;
 
 pub(crate) struct VelocitySolver {
-    pub mj_lambdas: Vec<DeltaVel<Real>>,
-    pub generic_mj_lambdas: DVector<Real>,
+    pub solver_bodies: Vec<SolverBody>,
+    pub solver_vels: Vec<SolverVel<Real>>,
+    pub solver_vels_increment: Vec<SolverVel<Real>>,
+    pub generic_solver_vels: DVector<Real>,
+    pub generic_solver_vels_increment: DVector<Real>,
+    pub multibody_roots: Vec<MultibodyLinkId>,
 }
 
 impl VelocitySolver {
     pub fn new() -> Self {
         Self {
-            mj_lambdas: Vec::new(),
-            generic_mj_lambdas: DVector::zeros(0),
+            solver_bodies: Vec::new(),
+            solver_vels: Vec::new(),
+            solver_vels_increment: Vec::new(),
+            generic_solver_vels: DVector::zeros(0),
+            generic_solver_vels_increment: DVector::zeros(0),
+            multibody_roots: Vec::new(),
         }
     }
 
-    pub fn solve(
-        &mut self,
+    pub fn init_constraints(
+        &self,
         island_id: usize,
-        params: &IntegrationParameters,
         islands: &IslandManager,
         bodies: &mut RigidBodySet,
         multibodies: &mut MultibodyJointSet,
         manifolds_all: &mut [&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
         joints_all: &mut [JointGraphEdge],
-        contact_constraints: &mut [AnyVelocityConstraint],
-        generic_contact_jacobians: &DVector<Real>,
-        joint_constraints: &mut [AnyJointVelocityConstraint],
-        generic_joint_jacobians: &DVector<Real>,
+        joint_indices: &[JointIndex],
+        contact_constraints: &mut SolverConstraintsSet<ContactConstraintTypes>,
+        joint_constraints: &mut SolverConstraintsSet<JointConstraintTypes>,
     ) {
-        self.mj_lambdas.clear();
-        self.mj_lambdas
-            .resize(islands.active_island(island_id).len(), DeltaVel::zero());
+        contact_constraints.init(
+            island_id,
+            islands,
+            bodies,
+            multibodies,
+            manifolds_all,
+            manifold_indices,
+        );
 
-        let total_multibodies_ndofs = multibodies.multibodies.iter().map(|m| m.1.ndofs()).sum();
-        self.generic_mj_lambdas = DVector::zeros(total_multibodies_ndofs);
+        joint_constraints.init(
+            island_id,
+            islands,
+            bodies,
+            multibodies,
+            joints_all,
+            joint_indices,
+        );
+    }
 
-        // Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc):
+    pub fn init_solver_velocities_and_solver_bodies(
+        &mut self,
+        params: &IntegrationParameters,
+        island_id: usize,
+        islands: &IslandManager,
+        bodies: &mut RigidBodySet,
+        multibodies: &mut MultibodyJointSet,
+    ) {
+        self.multibody_roots.clear();
+        self.solver_bodies.clear();
+        self.solver_bodies.resize(
+            islands.active_island(island_id).len(),
+            SolverBody::default(),
+        );
+
+        self.solver_vels_increment.clear();
+        self.solver_vels_increment
+            .resize(islands.active_island(island_id).len(), SolverVel::zero());
+        self.solver_vels.clear();
+        self.solver_vels
+            .resize(islands.active_island(island_id).len(), SolverVel::zero());
+
+        /*
+         * Initialize solver bodies and delta-velocities (`solver_vels_increment`) with external forces (gravity etc):
+         * NOTE: we compute this only once by neglecting changes of mass matrices.
+         */
+
+        // Assign solver ids to multibodies, and collect the relevant roots.
+        // And init solver_vels for rigidb-bodies.
+        let mut multibody_solver_id = 0;
         for handle in islands.active_island(island_id) {
             if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
                 let multibody = multibodies
@@ -50,196 +101,213 @@ impl VelocitySolver {
                     .unwrap();
 
                 if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                    let mut mj_lambdas = self
-                        .generic_mj_lambdas
-                        .rows_mut(multibody.solver_id, multibody.ndofs());
-                    mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
+                    multibody.solver_id = multibody_solver_id;
+                    multibody_solver_id += multibody.ndofs();
+                    self.multibody_roots.push(link);
                 }
             } else {
                 let rb = &bodies[*handle];
-                let dvel = &mut self.mj_lambdas[rb.ids.active_set_offset];
+                let solver_vel = &mut self.solver_vels[rb.ids.active_set_offset];
+                let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_offset];
+                let solver_body = &mut self.solver_bodies[rb.ids.active_set_offset];
+                solver_body.copy_from(rb);
 
                 // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
                 //       by the square root of the inertia tensor:
-                dvel.angular +=
+                solver_vel_incr.angular =
                     rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
-                dvel.linear +=
+                solver_vel_incr.linear =
                     rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
+
+                solver_vel.linear = rb.vels.linvel;
+                // PERF: can we avoid the call to effective_angular_inertia_sqrt?
+                solver_vel.angular = rb.mprops.effective_angular_inertia_sqrt() * rb.vels.angvel;
             }
         }
 
-        /*
-         * Solve constraints.
-         */
-        for i in 0..params.max_velocity_iterations {
-            let solve_friction = params.interleave_restitution_and_friction_resolution
-                && params.max_velocity_friction_iterations + i >= params.max_velocity_iterations;
+        // PERF: don’t reallocate at each iteration.
+        self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id);
+        self.generic_solver_vels = DVector::zeros(multibody_solver_id);
 
-            for constraint in &mut *joint_constraints {
-                constraint.solve(
-                    generic_joint_jacobians,
-                    &mut self.mj_lambdas[..],
-                    &mut self.generic_mj_lambdas,
-                );
-            }
+        // init solver_vels for multibodies.
+        for link in &self.multibody_roots {
+            let multibody = multibodies
+                .get_multibody_mut_internal(link.multibody)
+                .unwrap();
+            multibody.update_dynamics(params.dt, bodies);
+            multibody.update_acceleration(bodies);
 
-            for constraint in &mut *contact_constraints {
-                constraint.solve(
-                    generic_contact_jacobians,
-                    &mut self.mj_lambdas[..],
-                    &mut self.generic_mj_lambdas,
-                    true,
-                    false,
-                );
-            }
+            let mut solver_vels_incr = self
+                .generic_solver_vels_increment
+                .rows_mut(multibody.solver_id, multibody.ndofs());
+            let mut solver_vels = self
+                .generic_solver_vels
+                .rows_mut(multibody.solver_id, multibody.ndofs());
 
-            if solve_friction {
-                for constraint in &mut *contact_constraints {
-                    constraint.solve(
-                        generic_contact_jacobians,
-                        &mut self.mj_lambdas[..],
-                        &mut self.generic_mj_lambdas,
-                        false,
-                        true,
-                    );
-                }
-            }
+            solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0);
+            solver_vels.copy_from(&multibody.velocities);
         }
+    }
 
-        let remaining_friction_iterations =
-            if !params.interleave_restitution_and_friction_resolution {
-                params.max_velocity_friction_iterations
-            } else if params.max_velocity_friction_iterations > params.max_velocity_iterations {
-                params.max_velocity_friction_iterations - params.max_velocity_iterations
+    pub fn solve_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        num_solver_iterations: usize,
+        bodies: &mut RigidBodySet,
+        multibodies: &mut MultibodyJointSet,
+        contact_constraints: &mut SolverConstraintsSet<ContactConstraintTypes>,
+        joint_constraints: &mut SolverConstraintsSet<JointConstraintTypes>,
+    ) {
+        for small_step_id in 0..num_solver_iterations {
+            let is_last_small_step = small_step_id == num_solver_iterations - 1;
+
+            for (solver_vels, incr) in self
+                .solver_vels
+                .iter_mut()
+                .zip(self.solver_vels_increment.iter())
+            {
+                solver_vels.linear += incr.linear;
+                solver_vels.angular += incr.angular;
+            }
+
+            self.generic_solver_vels += &self.generic_solver_vels_increment;
+
+            /*
+             * Update & solve constraints.
+             */
+            joint_constraints.update(&params, multibodies, &self.solver_bodies);
+            contact_constraints.update(&params, small_step_id, multibodies, &self.solver_bodies);
+
+            joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels);
+            contact_constraints
+                .solve_restitution(&mut self.solver_vels, &mut self.generic_solver_vels);
+
+            let num_friction_iterations = if is_last_small_step {
+                params.num_friction_iteration_per_solver_iteration * num_solver_iterations
+                    - (num_solver_iterations - 1)
             } else {
-                0
+                1
             };
 
-        for _ in 0..remaining_friction_iterations {
-            for constraint in &mut *contact_constraints {
-                constraint.solve(
-                    generic_contact_jacobians,
-                    &mut self.mj_lambdas[..],
-                    &mut self.generic_mj_lambdas,
-                    false,
-                    true,
-                );
+            for _ in 0..num_friction_iterations {
+                contact_constraints
+                    .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
             }
-        }
 
+            /*
+             * Integrate positions.
+             */
+            self.integrate_positions(&params, is_last_small_step, bodies, multibodies);
+
+            /*
+             * Resolution without bias.
+             */
+            joint_constraints.solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
+            contact_constraints
+                .solve_restitution_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
+        }
+    }
+
+    pub fn integrate_positions(
+        &mut self,
+        params: &IntegrationParameters,
+        is_last_small_step: bool,
+        bodies: &mut RigidBodySet,
+        multibodies: &mut MultibodyJointSet,
+    ) {
         // Integrate positions.
+        for (solver_vels, solver_body) in self.solver_vels.iter().zip(self.solver_bodies.iter_mut())
+        {
+            let linvel = solver_vels.linear;
+            let angvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular);
+
+            let mut new_vels = RigidBodyVelocity { linvel, angvel };
+            new_vels = new_vels.apply_damping(params.dt, &solver_body.damping);
+            let new_pos =
+                new_vels.integrate(params.dt, &solver_body.position, &solver_body.local_com);
+            solver_body.integrated_vels += new_vels;
+            solver_body.position = new_pos;
+            solver_body.world_com = new_pos * solver_body.local_com;
+        }
+
+        // Integrate multibody positions.
+        for link in &self.multibody_roots {
+            let multibody = multibodies
+                .get_multibody_mut_internal(link.multibody)
+                .unwrap();
+            let solver_vels = self
+                .generic_solver_vels
+                .rows(multibody.solver_id, multibody.ndofs());
+            multibody.velocities.copy_from(&solver_vels);
+            multibody.integrate(params.dt);
+            // PERF: we could have a mode where it doesn’t write back to the `bodies` yet.
+            multibody.forward_kinematics(bodies, !is_last_small_step);
+
+            if !is_last_small_step {
+                // These are very expensive and not needed if we don’t
+                // have to run another step.
+                multibody.update_dynamics(params.dt, bodies);
+                multibody.update_acceleration(bodies);
+
+                let mut solver_vels_incr = self
+                    .generic_solver_vels_increment
+                    .rows_mut(multibody.solver_id, multibody.ndofs());
+                solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0);
+            }
+        }
+    }
+
+    pub fn writeback_bodies(
+        &mut self,
+        params: &IntegrationParameters,
+        num_solver_iterations: usize,
+        islands: &IslandManager,
+        island_id: usize,
+        bodies: &mut RigidBodySet,
+        multibodies: &mut MultibodyJointSet,
+    ) {
         for handle in islands.active_island(island_id) {
-            if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
+            let link = if self.multibody_roots.is_empty() {
+                None
+            } else {
+                multibodies.rigid_body_link(*handle).copied()
+            };
+
+            if let Some(link) = link {
                 let multibody = multibodies
                     .get_multibody_mut_internal(link.multibody)
                     .unwrap();
 
                 if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                    let mj_lambdas = self
-                        .generic_mj_lambdas
+                    let solver_vels = self
+                        .generic_solver_vels
                         .rows(multibody.solver_id, multibody.ndofs());
-                    let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations.
-                    multibody.velocities += mj_lambdas;
-                    multibody.integrate(params.dt);
-                    multibody.forward_kinematics(bodies, false);
-                    multibody.velocities = prev_vels;
+                    multibody.velocities.copy_from(&solver_vels);
                 }
             } else {
                 let rb = bodies.index_mut_internal(*handle);
+                let solver_body = &self.solver_bodies[rb.ids.active_set_offset];
+                let solver_vels = &self.solver_vels[rb.ids.active_set_offset];
 
-                let dvel = self.mj_lambdas[rb.ids.active_set_offset];
-                let dangvel = rb
-                    .mprops
-                    .effective_world_inv_inertia_sqrt
-                    .transform_vector(dvel.angular);
+                let dangvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular);
 
-                // Update positions.
-                let mut new_pos = rb.pos;
-                let mut new_vels = rb.vels;
-                new_vels.linvel += dvel.linear;
-                new_vels.angvel += dangvel;
-                new_vels = new_vels.apply_damping(params.dt, &rb.damping);
-                new_pos.next_position = new_vels.integrate(
-                    params.dt,
-                    &rb.pos.position,
-                    &rb.mprops.local_mprops.local_com,
-                );
-                rb.integrated_vels = new_vels;
-                rb.pos = new_pos;
+                let mut new_vels = RigidBodyVelocity {
+                    linvel: solver_vels.linear,
+                    angvel: dangvel,
+                };
+                new_vels = new_vels.apply_damping(params.dt, &solver_body.damping);
+
+                // NOTE: using integrated_vels instead of interpolation is interesting for
+                //       high angular velocities. However, it is a bit inexact due to the
+                //       solver integrating at intermediate sub-steps. Should we just switch
+                //       to interpolation?
+                rb.integrated_vels.linvel =
+                    solver_body.integrated_vels.linvel / num_solver_iterations as Real;
+                rb.integrated_vels.angvel =
+                    solver_body.integrated_vels.angvel / num_solver_iterations as Real;
+                rb.vels = new_vels;
+                rb.pos.next_position = solver_body.position;
             }
         }
-
-        for joint in &mut *joint_constraints {
-            joint.remove_bias_from_rhs();
-        }
-        for constraint in &mut *contact_constraints {
-            constraint.remove_bias_from_rhs();
-        }
-
-        for _ in 0..params.max_stabilization_iterations {
-            for constraint in &mut *joint_constraints {
-                constraint.solve(
-                    generic_joint_jacobians,
-                    &mut self.mj_lambdas[..],
-                    &mut self.generic_mj_lambdas,
-                );
-            }
-
-            for constraint in &mut *contact_constraints {
-                constraint.solve(
-                    generic_contact_jacobians,
-                    &mut self.mj_lambdas[..],
-                    &mut self.generic_mj_lambdas,
-                    true,
-                    false,
-                );
-            }
-
-            for constraint in &mut *contact_constraints {
-                constraint.solve(
-                    generic_contact_jacobians,
-                    &mut self.mj_lambdas[..],
-                    &mut self.generic_mj_lambdas,
-                    false,
-                    true,
-                );
-            }
-        }
-
-        // Update velocities.
-        for handle in islands.active_island(island_id) {
-            if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
-                let multibody = multibodies
-                    .get_multibody_mut_internal(link.multibody)
-                    .unwrap();
-
-                if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                    let mj_lambdas = self
-                        .generic_mj_lambdas
-                        .rows(multibody.solver_id, multibody.ndofs());
-                    multibody.velocities += mj_lambdas;
-                }
-            } else {
-                let rb = bodies.index_mut_internal(*handle);
-                let dvel = self.mj_lambdas[rb.ids.active_set_offset];
-                let dangvel = rb
-                    .mprops
-                    .effective_world_inv_inertia_sqrt
-                    .transform_vector(dvel.angular);
-
-                rb.vels.linvel += dvel.linear;
-                rb.vels.angvel += dangvel;
-                rb.vels = rb.vels.apply_damping(params.dt, &rb.damping);
-            }
-        }
-
-        // Write impulses back into the manifold structures.
-        for constraint in &*joint_constraints {
-            constraint.writeback_impulses(joints_all);
-        }
-
-        for constraint in &*contact_constraints {
-            constraint.writeback_impulses(manifolds_all);
-        }
     }
 }