Implement multibody joints and the new solver

src/dynamics/solver/categorization.rs

@@ -1,18 +1,33 @@
 use crate::data::ComponentSet;
-use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodyType};
+use crate::dynamics::{JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyType};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub(crate) fn categorize_contacts(
     _bodies: &impl ComponentSet<RigidBodyType>, // Unused but useful to simplify the parallel code.
+    multibody_joints: &MultibodyJointSet,
     manifolds: &[&mut ContactManifold],
     manifold_indices: &[ContactManifoldIndex],
     out_ground: &mut Vec<ContactManifoldIndex>,
     out_not_ground: &mut Vec<ContactManifoldIndex>,
+    out_generic: &mut Vec<ContactManifoldIndex>,
+    _unused: &mut Vec<ContactManifoldIndex>, // Unused but useful to simplify the parallel code.
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
 
-        if manifold.data.relative_dominance != 0 {
+        if manifold
+            .data
+            .rigid_body1
+            .map(|rb| multibody_joints.rigid_body_link(rb))
+            .is_some()
+            || manifold
+                .data
+                .rigid_body1
+                .map(|rb| multibody_joints.rigid_body_link(rb))
+                .is_some()
+        {
+            out_generic.push(*manifold_i);
+        } else if manifold.data.relative_dominance != 0 {
             out_ground.push(*manifold_i)
         } else {
             out_not_ground.push(*manifold_i)
@@ -22,17 +37,28 @@ pub(crate) fn categorize_contacts(
 
 pub(crate) fn categorize_joints(
     bodies: &impl ComponentSet<RigidBodyType>,
-    joints: &[JointGraphEdge],
+    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>,
 ) {
     for joint_i in joint_indices {
-        let joint = &joints[*joint_i].weight;
+        let joint = &impulse_joints[*joint_i].weight;
         let status1 = bodies.index(joint.body1.0);
         let status2 = bodies.index(joint.body2.0);
 
-        if !status1.is_dynamic() || !status2.is_dynamic() {
+        if multibody_joints.rigid_body_link(joint.body1).is_some()
+            || multibody_joints.rigid_body_link(joint.body2).is_some()
+        {
+            if !status1.is_dynamic() || !status2.is_dynamic() {
+                generic_ground_joints.push(*joint_i);
+            } else {
+                generic_nonground_joints.push(*joint_i);
+            }
+        } else if !status1.is_dynamic() || !status2.is_dynamic() {
             ground_joints.push(*joint_i);
         } else {
             nonground_joints.push(*joint_i);

src/dynamics/solver/delta_vel.rs

@@ -1,14 +1,34 @@
-use crate::math::{AngVector, Vector};
+use crate::math::{AngVector, Vector, SPATIAL_DIM};
+use na::{DVectorSlice, DVectorSliceMut};
 use na::{Scalar, SimdRealField};
 use std::ops::AddAssign;
 
 #[derive(Copy, Clone, Debug)]
+#[repr(C)]
 //#[repr(align(64))]
-pub(crate) struct DeltaVel<N: Scalar + Copy> {
+pub struct DeltaVel<N: Scalar + Copy> {
     pub linear: Vector<N>,
     pub angular: AngVector<N>,
 }
 
+impl<N: Scalar + Copy> DeltaVel<N> {
+    pub fn as_slice(&self) -> &[N; SPATIAL_DIM] {
+        unsafe { std::mem::transmute(self) }
+    }
+
+    pub fn as_mut_slice(&mut self) -> &mut [N; SPATIAL_DIM] {
+        unsafe { std::mem::transmute(self) }
+    }
+
+    pub fn as_vector_slice(&self) -> DVectorSlice<N> {
+        DVectorSlice::from_slice(&self.as_slice()[..], SPATIAL_DIM)
+    }
+
+    pub fn as_vector_slice_mut(&mut self) -> DVectorSliceMut<N> {
+        DVectorSliceMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM)
+    }
+}
+
 impl<N: SimdRealField + Copy> DeltaVel<N> {
     pub fn zero() -> Self {
         Self {

src/dynamics/solver/generic_velocity_constraint.rs

@@ -0,0 +1,377 @@
+use crate::data::{BundleSet, ComponentSet};
+use crate::dynamics::solver::{GenericRhs, VelocityConstraint};
+use crate::dynamics::{
+    IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType,
+    RigidBodyVelocity,
+};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
+use crate::utils::{WAngularInertia, WCross, WDot};
+
+use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
+#[cfg(feature = "dim2")]
+use crate::utils::WBasis;
+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,
+}
+
+impl GenericVelocityConstraint {
+    pub fn generate<Bodies>(
+        params: &IntegrationParameters,
+        manifold_id: ContactManifoldIndex,
+        manifold: &ContactManifold,
+        bodies: &Bodies,
+        multibodies: &MultibodyJointSet,
+        out_constraints: &mut Vec<GenericVelocityConstraint>,
+        jacobians: &mut DVector<Real>,
+        jacobian_id: &mut usize,
+        push: bool,
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyType>,
+    {
+        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 (rb_ids1, rb_vels1, rb_mprops1, rb_type1): (
+            &RigidBodyIds,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyType,
+        ) = bodies.index_bundle(handle1.0);
+        let (rb_ids2, rb_vels2, rb_mprops2, rb_type2): (
+            &RigidBodyIds,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyType,
+        ) = bodies.index_bundle(handle2.0);
+
+        let multibody1 = multibodies
+            .rigid_body_link(handle1)
+            .map(|m| (&multibodies[m.multibody], m.id));
+        let multibody2 = multibodies
+            .rigid_body_link(handle2)
+            .map(|m| (&multibodies[m.multibody], m.id));
+        let mj_lambda1 = multibody1
+            .map(|mb| mb.0.solver_id)
+            .unwrap_or(if rb_type1.is_dynamic() {
+                rb_ids1.active_set_offset
+            } else {
+                0
+            });
+        let mj_lambda2 = multibody2
+            .map(|mb| mb.0.solver_id)
+            .unwrap_or(if rb_type2.is_dynamic() {
+                rb_ids2.active_set_offset
+            } else {
+                0
+            });
+        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,
+            &rb_vels1.linvel,
+            &rb_vels2.linvel,
+        );
+
+        let multibodies_ndof = multibody1.map(|m| m.0.ndofs()).unwrap_or(0)
+            + multibody2.map(|m| m.0.ndofs()).unwrap_or(0);
+        // 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 {
+            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 constraint = VelocityConstraint {
+                dir1: force_dir1,
+                #[cfg(feature = "dim3")]
+                tangent1: tangents1[0],
+                elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+                im1: if rb_type1.is_dynamic() {
+                    rb_mprops1.effective_inv_mass
+                } else {
+                    0.0
+                },
+                im2: if rb_type2.is_dynamic() {
+                    rb_mprops2.effective_inv_mass
+                } else {
+                    0.0
+                },
+                limit: 0.0,
+                mj_lambda1,
+                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 - rb_mprops1.world_com;
+                let dp2 = manifold_point.point - rb_mprops2.world_com;
+
+                let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1);
+                let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2);
+
+                constraint.limit = manifold_point.friction;
+                constraint.manifold_contact_id[k] = manifold_point.contact_id;
+
+                // Normal part.
+                {
+                    let torque_dir1 = dp1.gcross(force_dir1);
+                    let torque_dir2 = dp2.gcross(-force_dir1);
+
+                    let gcross1 = if rb_type1.is_dynamic() {
+                        rb_mprops1
+                            .effective_world_inv_inertia_sqrt
+                            .transform_vector(torque_dir1)
+                    } else {
+                        na::zero()
+                    };
+                    let gcross2 = if rb_type2.is_dynamic() {
+                        rb_mprops2
+                            .effective_world_inv_inertia_sqrt
+                            .transform_vector(torque_dir2)
+                    } else {
+                        na::zero()
+                    };
+
+                    let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() {
+                        mb1.fill_jacobians(
+                            *link_id1,
+                            force_dir1,
+                            #[cfg(feature = "dim2")]
+                            na::vector!(torque_dir1),
+                            #[cfg(feature = "dim3")]
+                            torque_dir1,
+                            jacobian_id,
+                            jacobians,
+                        )
+                        .0
+                    } else if rb_type1.is_dynamic() {
+                        rb_mprops1.effective_inv_mass + gcross1.gdot(gcross1)
+                    } else {
+                        0.0
+                    };
+
+                    let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() {
+                        mb2.fill_jacobians(
+                            *link_id2,
+                            -force_dir1,
+                            #[cfg(feature = "dim2")]
+                            na::vector!(torque_dir2),
+                            #[cfg(feature = "dim3")]
+                            torque_dir2,
+                            jacobian_id,
+                            jacobians,
+                        )
+                        .0
+                    } else if rb_type2.is_dynamic() {
+                        rb_mprops2.effective_inv_mass + gcross2.gdot(gcross2)
+                    } else {
+                        0.0
+                    };
+
+                    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 * params.velocity_solve_fraction;
+                    let rhs_bias =
+                        /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
+
+                    constraint.elements[k].normal_part = VelocityConstraintNormalPart {
+                        gcross1,
+                        gcross2,
+                        rhs: rhs_wo_bias + rhs_bias,
+                        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_dir1 = dp1.gcross(tangents1[j]);
+                        let gcross1 = if rb_type1.is_dynamic() {
+                            rb_mprops1
+                                .effective_world_inv_inertia_sqrt
+                                .transform_vector(torque_dir1)
+                        } else {
+                            na::zero()
+                        };
+                        constraint.elements[k].tangent_part.gcross1[j] = gcross1;
+
+                        let torque_dir2 = dp2.gcross(-tangents1[j]);
+                        let gcross2 = if rb_type2.is_dynamic() {
+                            rb_mprops2
+                                .effective_world_inv_inertia_sqrt
+                                .transform_vector(torque_dir2)
+                        } else {
+                            na::zero()
+                        };
+                        constraint.elements[k].tangent_part.gcross2[j] = gcross2;
+
+                        let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() {
+                            mb1.fill_jacobians(
+                                *link_id1,
+                                tangents1[j],
+                                #[cfg(feature = "dim2")]
+                                na::vector![torque_dir1],
+                                #[cfg(feature = "dim3")]
+                                torque_dir1,
+                                jacobian_id,
+                                jacobians,
+                            )
+                            .0
+                        } else if rb_type1.is_dynamic() {
+                            rb_mprops1.effective_inv_mass + gcross1.gdot(gcross1)
+                        } else {
+                            0.0
+                        };
+
+                        let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() {
+                            mb2.fill_jacobians(
+                                *link_id2,
+                                -tangents1[j],
+                                #[cfg(feature = "dim2")]
+                                na::vector![torque_dir2],
+                                #[cfg(feature = "dim3")]
+                                torque_dir2,
+                                jacobian_id,
+                                jacobians,
+                            )
+                            .0
+                        } else if rb_type2.is_dynamic() {
+                            rb_mprops2.effective_inv_mass + gcross2.gdot(gcross2)
+                        } else {
+                            0.0
+                        };
+
+                        let r = crate::utils::inv(inv_r1 + inv_r2);
+
+                        let rhs =
+                            (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
+
+                        constraint.elements[k].tangent_part.rhs[j] = rhs;
+                        constraint.elements[k].tangent_part.r[j] = r;
+                    }
+                }
+            }
+
+            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)
+                | ((multibody2.is_some() as u8) << 1)
+                | (!rb_type1.is_dynamic() as u8)
+                | ((!rb_type2.is_dynamic() as u8) << 1);
+
+            let constraint = GenericVelocityConstraint {
+                velocity_constraint: constraint,
+                j_id: chunk_j_id,
+                ndofs1,
+                ndofs2,
+                generic_constraint_mask,
+            };
+
+            if push {
+                out_constraints.push(constraint);
+            } else {
+                out_constraints[manifold.data.constraint_index + _l] = constraint;
+            }
+        }
+    }
+
+    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,
+    ) {
+        let mut mj_lambda1 = if self.generic_constraint_mask & 0b01 == 0 {
+            GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda1 as usize])
+        } else {
+            GenericRhs::GenericId(self.velocity_constraint.mj_lambda1 as usize)
+        };
+
+        let mut mj_lambda2 = if self.generic_constraint_mask & 0b10 == 0 {
+            GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda2 as usize])
+        } else {
+            GenericRhs::GenericId(self.velocity_constraint.mj_lambda2 as usize)
+        };
+
+        let elements = &mut self.velocity_constraint.elements
+            [..self.velocity_constraint.num_contacts as usize];
+        VelocityConstraintElement::generic_solve_group(
+            elements,
+            jacobians,
+            &self.velocity_constraint.dir1,
+            #[cfg(feature = "dim3")]
+            &self.velocity_constraint.tangent1,
+            self.velocity_constraint.im1,
+            self.velocity_constraint.im2,
+            self.velocity_constraint.limit,
+            self.ndofs1,
+            self.ndofs2,
+            self.j_id,
+            &mut mj_lambda1,
+            &mut mj_lambda2,
+            generic_mj_lambdas,
+            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::DeltaVel(mj_lambda2) = mj_lambda2 {
+            mj_lambdas[self.velocity_constraint.mj_lambda2 as usize] = mj_lambda2;
+        }
+    }
+
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        self.velocity_constraint.writeback_impulses(manifolds_all);
+    }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        self.velocity_constraint.remove_bias_from_rhs();
+    }
+}

src/dynamics/solver/generic_velocity_constraint_element.rs

@@ -0,0 +1,348 @@
+use super::DeltaVel;
+use crate::dynamics::solver::{
+    VelocityConstraintElement, VelocityConstraintNormalPart, VelocityConstraintTangentPart,
+};
+use crate::math::{AngVector, Real, Vector, DIM};
+use crate::utils::WDot;
+use na::DVector;
+#[cfg(feature = "dim2")]
+use {crate::utils::WBasis, na::SimdPartialOrd};
+
+pub(crate) enum GenericRhs {
+    DeltaVel(DeltaVel<Real>),
+    GenericId(usize),
+}
+
+// Offset between the jacobians of two consecutive constraints.
+#[inline(always)]
+fn j_step(ndofs1: usize, ndofs2: usize) -> usize {
+    (ndofs1 + ndofs2) * 2
+}
+
+#[inline(always)]
+fn j_id1(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize {
+    j_id
+}
+
+#[inline(always)]
+fn j_id2(j_id: usize, ndofs1: usize, _ndofs2: usize) -> usize {
+    j_id + ndofs1 * 2
+}
+
+#[inline(always)]
+fn normal_j_id(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize {
+    j_id
+}
+
+#[inline(always)]
+fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize {
+    j_id + (ndofs1 + ndofs2) * 2
+}
+
+impl GenericRhs {
+    #[inline(always)]
+    fn dimpulse(
+        &self,
+        j_id: usize,
+        ndofs: usize,
+        jacobians: &DVector<Real>,
+        dir: &Vector<Real>,
+        gcross: &AngVector<Real>,
+        mj_lambdas: &DVector<Real>,
+    ) -> Real {
+        match self {
+            GenericRhs::DeltaVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular),
+            GenericRhs::GenericId(mj_lambda) => {
+                let j = jacobians.rows(j_id, ndofs);
+                let rhs = mj_lambdas.rows(*mj_lambda, ndofs);
+                j.dot(&rhs)
+            }
+        }
+    }
+
+    #[inline(always)]
+    fn apply_impulse(
+        &mut self,
+        j_id: usize,
+        ndofs: usize,
+        impulse: Real,
+        jacobians: &DVector<Real>,
+        dir: &Vector<Real>,
+        gcross: &AngVector<Real>,
+        mj_lambdas: &mut DVector<Real>,
+        inv_mass: Real,
+    ) {
+        match self {
+            GenericRhs::DeltaVel(rhs) => {
+                rhs.linear += dir * (inv_mass * impulse);
+                rhs.angular += gcross * impulse;
+            }
+            GenericRhs::GenericId(mj_lambda) => {
+                let wj_id = j_id + ndofs;
+                let wj = jacobians.rows(wj_id, ndofs);
+                let mut rhs = mj_lambdas.rows_mut(*mj_lambda, ndofs);
+                rhs.axpy(impulse, &wj, 1.0);
+            }
+        }
+    }
+}
+
+impl VelocityConstraintTangentPart<Real> {
+    #[inline]
+    pub fn generic_solve(
+        &mut self,
+        j_id: usize,
+        jacobians: &DVector<Real>,
+        tangents1: [&Vector<Real>; DIM - 1],
+        im1: Real,
+        im2: Real,
+        ndofs1: usize,
+        ndofs2: usize,
+        limit: Real,
+        mj_lambda1: &mut GenericRhs,
+        mj_lambda2: &mut GenericRhs,
+        mj_lambdas: &mut DVector<Real>,
+    ) {
+        let j_id1 = j_id1(j_id, ndofs1, ndofs2);
+        let j_id2 = j_id2(j_id, ndofs1, ndofs2);
+        #[cfg(feature = "dim3")]
+        let j_step = j_step(ndofs1, ndofs2);
+
+        #[cfg(feature = "dim2")]
+        {
+            let dimpulse_0 = mj_lambda1.dimpulse(
+                j_id1,
+                ndofs1,
+                jacobians,
+                &tangents1[0],
+                &self.gcross1[0],
+                mj_lambdas,
+            ) + mj_lambda2.dimpulse(
+                j_id2,
+                ndofs2,
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                mj_lambdas,
+            ) + self.rhs[0];
+
+            let new_impulse = (self.impulse[0] - self.r[0] * dimpulse_0).simd_clamp(-limit, limit);
+            let dlambda = new_impulse - self.impulse[0];
+            self.impulse[0] = new_impulse;
+
+            mj_lambda1.apply_impulse(
+                j_id1,
+                ndofs1,
+                dlambda,
+                jacobians,
+                &tangents1[0],
+                &self.gcross1[0],
+                mj_lambdas,
+                im1,
+            );
+            mj_lambda2.apply_impulse(
+                j_id2,
+                ndofs2,
+                dlambda,
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                mj_lambdas,
+                im2,
+            );
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let dimpulse_0 = mj_lambda1.dimpulse(
+                j_id1,
+                ndofs1,
+                jacobians,
+                &tangents1[0],
+                &self.gcross1[0],
+                mj_lambdas,
+            ) + mj_lambda2.dimpulse(
+                j_id2,
+                ndofs2,
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                mj_lambdas,
+            ) + self.rhs[0];
+            let dimpulse_1 = mj_lambda1.dimpulse(
+                j_id1 + j_step,
+                ndofs1,
+                jacobians,
+                &tangents1[1],
+                &self.gcross1[1],
+                mj_lambdas,
+            ) + mj_lambda2.dimpulse(
+                j_id2 + j_step,
+                ndofs2,
+                jacobians,
+                &-tangents1[1],
+                &self.gcross2[1],
+                mj_lambdas,
+            ) + self.rhs[1];
+
+            let new_impulse = na::Vector2::new(
+                self.impulse[0] - self.r[0] * dimpulse_0,
+                self.impulse[1] - self.r[1] * dimpulse_1,
+            );
+            let new_impulse = new_impulse.cap_magnitude(limit);
+
+            let dlambda = new_impulse - self.impulse;
+            self.impulse = new_impulse;
+
+            mj_lambda1.apply_impulse(
+                j_id1,
+                ndofs1,
+                dlambda[0],
+                jacobians,
+                &tangents1[0],
+                &self.gcross1[0],
+                mj_lambdas,
+                im1,
+            );
+            mj_lambda1.apply_impulse(
+                j_id1 + j_step,
+                ndofs1,
+                dlambda[1],
+                jacobians,
+                &tangents1[1],
+                &self.gcross1[1],
+                mj_lambdas,
+                im1,
+            );
+
+            mj_lambda2.apply_impulse(
+                j_id2,
+                ndofs2,
+                dlambda[0],
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                mj_lambdas,
+                im2,
+            );
+            mj_lambda2.apply_impulse(
+                j_id2 + j_step,
+                ndofs2,
+                dlambda[1],
+                jacobians,
+                &-tangents1[1],
+                &self.gcross2[1],
+                mj_lambdas,
+                im2,
+            );
+        }
+    }
+}
+
+impl VelocityConstraintNormalPart<Real> {
+    #[inline]
+    pub fn generic_solve(
+        &mut self,
+        j_id: usize,
+        jacobians: &DVector<Real>,
+        dir1: &Vector<Real>,
+        im1: Real,
+        im2: Real,
+        ndofs1: usize,
+        ndofs2: usize,
+        mj_lambda1: &mut GenericRhs,
+        mj_lambda2: &mut GenericRhs,
+        mj_lambdas: &mut DVector<Real>,
+    ) {
+        let j_id1 = j_id1(j_id, ndofs1, ndofs2);
+        let j_id2 = j_id2(j_id, ndofs1, ndofs2);
+
+        let dimpulse =
+            mj_lambda1.dimpulse(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
+                + mj_lambda2.dimpulse(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
+                + self.rhs;
+
+        let new_impulse = (self.impulse - self.r * dimpulse).max(0.0);
+        let dlambda = new_impulse - self.impulse;
+        self.impulse = new_impulse;
+
+        mj_lambda1.apply_impulse(
+            j_id1,
+            ndofs1,
+            dlambda,
+            jacobians,
+            &dir1,
+            &self.gcross1,
+            mj_lambdas,
+            im1,
+        );
+        mj_lambda2.apply_impulse(
+            j_id2,
+            ndofs2,
+            dlambda,
+            jacobians,
+            &-dir1,
+            &self.gcross2,
+            mj_lambdas,
+            im2,
+        );
+    }
+}
+
+impl VelocityConstraintElement<Real> {
+    #[inline]
+    pub fn generic_solve_group(
+        elements: &mut [Self],
+        jacobians: &DVector<Real>,
+        dir1: &Vector<Real>,
+        #[cfg(feature = "dim3")] tangent1: &Vector<Real>,
+        im1: Real,
+        im2: Real,
+        limit: Real,
+        // ndofs is 0 for a non-multibody body, or a multibody with zero
+        // degrees of freedom.
+        ndofs1: usize,
+        ndofs2: usize,
+        // Jacobian index of the first constraint.
+        j_id: usize,
+        mj_lambda1: &mut GenericRhs,
+        mj_lambda2: &mut GenericRhs,
+        mj_lambdas: &mut DVector<Real>,
+        solve_restitution: bool,
+        solve_friction: bool,
+    ) {
+        let j_step = j_step(ndofs1, ndofs2) * DIM;
+
+        // Solve penetration.
+        if solve_restitution {
+            let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
+
+            for element in elements.iter_mut() {
+                element.normal_part.generic_solve(
+                    nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1, mj_lambda2,
+                    mj_lambdas,
+                );
+                nrm_j_id += j_step;
+            }
+        }
+
+        // Solve friction.
+        if solve_friction {
+            #[cfg(feature = "dim3")]
+            let tangents1 = [tangent1, &dir1.cross(&tangent1)];
+            #[cfg(feature = "dim2")]
+            let tangents1 = [&dir1.orthonormal_vector()];
+            let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
+
+            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, tangents1, im1, im2, ndofs1, ndofs2, limit, mj_lambda1,
+                    mj_lambda2, mj_lambdas,
+                );
+                tng_j_id += j_step;
+            }
+        }
+    }
+}

src/dynamics/solver/interaction_groups.rs

@@ -216,12 +216,13 @@ impl InteractionGroups {
     ) where
         Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
     {
-        // NOTE: in 3D we have up to 10 different joint types.
-        // In 2D we only have 5 joint types.
+        // TODO: right now, we only sort based on the axes locked by the joint.
+        // We could also take motors and limits into account in the future (most of
+        // the SIMD constraints generation for motors and limits is already implemented).
         #[cfg(feature = "dim3")]
-        const NUM_JOINT_TYPES: usize = 10;
+        const NUM_JOINT_TYPES: usize = 64;
         #[cfg(feature = "dim2")]
-        const NUM_JOINT_TYPES: usize = 5;
+        const NUM_JOINT_TYPES: usize = 8;
 
         // The j-th bit of joint_type_conflicts[i] indicates that the
         // j-th bucket contains a joint with a type different than `i`.
@@ -254,13 +255,13 @@ impl InteractionGroups {
                 continue;
             }
 
-            if !interaction.supports_simd_constraints() {
+            if !interaction.data.supports_simd_constraints() {
                 // This joint does not support simd constraints yet.
                 self.nongrouped_interactions.push(*interaction_i);
                 continue;
             }
 
-            let ijoint = interaction.params.type_id();
+            let ijoint = interaction.data.locked_axes.bits() as usize;
             let i1 = ids1.active_set_offset;
             let i2 = ids2.active_set_offset;
             let conflicts =

src/dynamics/solver/island_solver.rs

@@ -1,9 +1,8 @@
-use super::{PositionSolver, VelocitySolver};
+use super::VelocitySolver;
 use crate::counters::Counters;
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
 use crate::dynamics::solver::{
-    AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
-    AnyVelocityConstraint, SolverConstraints,
+    AnyJointVelocityConstraint, AnyVelocityConstraint, GenericVelocityConstraint, SolverConstraints,
 };
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
@@ -11,12 +10,12 @@ use crate::dynamics::{
 };
 use crate::dynamics::{IslandManager, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::prelude::{MultibodyJointSet, RigidBodyActivation};
 
 pub struct IslandSolver {
-    contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>,
-    joint_constraints: SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>,
+    contact_constraints: SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>,
+    joint_constraints: SolverConstraints<AnyJointVelocityConstraint, ()>,
     velocity_solver: VelocitySolver,
-    position_solver: PositionSolver,
 }
 
 impl Default for IslandSolver {
@@ -31,33 +30,10 @@ impl IslandSolver {
             contact_constraints: SolverConstraints::new(),
             joint_constraints: SolverConstraints::new(),
             velocity_solver: VelocitySolver::new(),
-            position_solver: PositionSolver::new(),
         }
     }
 
-    pub fn solve_position_constraints<Bodies>(
-        &mut self,
-        island_id: usize,
-        islands: &IslandManager,
-        counters: &mut Counters,
-        params: &IntegrationParameters,
-        bodies: &mut Bodies,
-    ) where
-        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
-    {
-        counters.solver.position_resolution_time.resume();
-        self.position_solver.solve(
-            island_id,
-            params,
-            islands,
-            bodies,
-            &self.contact_constraints.position_constraints,
-            &self.joint_constraints.position_constraints,
-        );
-        counters.solver.position_resolution_time.pause();
-    }
-
-    pub fn init_constraints_and_solve_velocity_constraints<Bodies>(
+    pub fn init_and_solve<Bodies>(
         &mut self,
         island_id: usize,
         counters: &mut Counters,
@@ -66,31 +42,64 @@ impl IslandSolver {
         bodies: &mut Bodies,
         manifolds: &mut [&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
-        joints: &mut [JointGraphEdge],
+        impulse_joints: &mut [JointGraphEdge],
         joint_indices: &[JointIndex],
+        multibody_joints: &mut MultibodyJointSet,
     ) where
         Bodies: ComponentSet<RigidBodyForces>
             + ComponentSetMut<RigidBodyPosition>
             + ComponentSetMut<RigidBodyVelocity>
-            + ComponentSet<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyActivation>
             + ComponentSet<RigidBodyDamping>
             + ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyType>,
     {
-        let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0;
+        let mut has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0;
+        if !has_constraints {
+            // Check if the multibody_joints have internal constraints.
+            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.has_active_internal_constraints()
+                    {
+                        has_constraints = true;
+                        break;
+                    }
+                }
+            }
+        }
 
         if has_constraints {
+            // 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_assembly_time.resume();
             self.contact_constraints.init(
                 island_id,
                 params,
                 islands,
                 bodies,
+                multibody_joints,
                 manifolds,
                 manifold_indices,
             );
-            self.joint_constraints
-                .init(island_id, params, islands, bodies, joints, joint_indices);
+            self.joint_constraints.init(
+                island_id,
+                params,
+                islands,
+                bodies,
+                multibody_joints,
+                impulse_joints,
+                joint_indices,
+            );
             counters.solver.velocity_assembly_time.pause();
 
             counters.solver.velocity_resolution_time.resume();
@@ -99,57 +108,53 @@ impl IslandSolver {
                 params,
                 islands,
                 bodies,
+                multibody_joints,
                 manifolds,
-                joints,
+                impulse_joints,
                 &mut self.contact_constraints.velocity_constraints,
+                &mut self.contact_constraints.generic_velocity_constraints,
+                &self.contact_constraints.generic_jacobians,
                 &mut self.joint_constraints.velocity_constraints,
+                &self.joint_constraints.generic_jacobians,
             );
             counters.solver.velocity_resolution_time.pause();
-
-            counters.solver.velocity_update_time.resume();
-
-            for handle in islands.active_island(island_id) {
-                let (poss, vels, damping, mprops): (
-                    &RigidBodyPosition,
-                    &RigidBodyVelocity,
-                    &RigidBodyDamping,
-                    &RigidBodyMassProps,
-                ) = bodies.index_bundle(handle.0);
-
-                let mut new_poss = *poss;
-                let new_vels = vels.apply_damping(params.dt, damping);
-                new_poss.next_position =
-                    vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com);
-
-                bodies.set_internal(handle.0, new_vels);
-                bodies.set_internal(handle.0, new_poss);
-            }
-
-            counters.solver.velocity_update_time.pause();
         } else {
             self.contact_constraints.clear();
             self.joint_constraints.clear();
             counters.solver.velocity_update_time.resume();
 
             for handle in islands.active_island(island_id) {
-                // Since we didn't run the velocity solver we need to integrate the accelerations here
-                let (poss, vels, forces, damping, mprops): (
-                    &RigidBodyPosition,
-                    &RigidBodyVelocity,
-                    &RigidBodyForces,
-                    &RigidBodyDamping,
-                    &RigidBodyMassProps,
-                ) = bodies.index_bundle(handle.0);
+                if let Some(link) = multibody_joints.rigid_body_link(*handle).copied() {
+                    let multibody = multibody_joints
+                        .get_multibody_mut_internal(link.multibody)
+                        .unwrap();
 
-                let mut new_poss = *poss;
-                let new_vels = forces
-                    .integrate(params.dt, vels, mprops)
-                    .apply_damping(params.dt, &damping);
-                new_poss.next_position =
-                    vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com);
+                    if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
+                        let accels = &multibody.accelerations;
+                        multibody.velocities.axpy(params.dt, accels, 1.0);
+                        multibody.integrate_next(params.dt);
+                        multibody.forward_kinematics_next(bodies, false);
+                    }
+                } else {
+                    // Since we didn't run the velocity solver we need to integrate the accelerations here
+                    let (poss, vels, forces, damping, mprops): (
+                        &RigidBodyPosition,
+                        &RigidBodyVelocity,
+                        &RigidBodyForces,
+                        &RigidBodyDamping,
+                        &RigidBodyMassProps,
+                    ) = bodies.index_bundle(handle.0);
 
-                bodies.set_internal(handle.0, new_vels);
-                bodies.set_internal(handle.0, new_poss);
+                    let mut new_poss = *poss;
+                    let new_vels = forces
+                        .integrate(params.dt, vels, mprops)
+                        .apply_damping(params.dt, &damping);
+                    new_poss.next_position =
+                        vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com);
+
+                    bodies.set_internal(handle.0, new_vels);
+                    bodies.set_internal(handle.0, new_poss);
+                }
             }
             counters.solver.velocity_update_time.pause();
         }

src/dynamics/solver/joint_constraint/ball_position_constraint.rs

@@ -1,266 +0,0 @@
-use crate::dynamics::{
-    BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-};
-#[cfg(feature = "dim2")]
-use crate::math::SdpMatrix;
-use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, UnitVector};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-
-#[derive(Debug)]
-pub(crate) struct BallPositionConstraint {
-    position1: usize,
-    position2: usize,
-
-    local_com1: Point<Real>,
-    local_com2: Point<Real>,
-
-    im1: Real,
-    im2: Real,
-
-    ii1: AngularInertia<Real>,
-    ii2: AngularInertia<Real>,
-    inv_ii1_ii2: AngularInertia<Real>,
-
-    local_anchor1: Point<Real>,
-    local_anchor2: Point<Real>,
-
-    limits_enabled: bool,
-    limits_angle: Real,
-    limits_local_axis1: UnitVector<Real>,
-    limits_local_axis2: UnitVector<Real>,
-}
-
-impl BallPositionConstraint {
-    pub fn from_params(
-        rb1: (&RigidBodyMassProps, &RigidBodyIds),
-        rb2: (&RigidBodyMassProps, &RigidBodyIds),
-        cparams: &BallJoint,
-    ) -> Self {
-        let (mprops1, ids1) = rb1;
-        let (mprops2, ids2) = rb2;
-
-        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let inv_ii1_ii2 = (ii1 + ii2).inverse();
-
-        Self {
-            local_com1: mprops1.local_mprops.local_com,
-            local_com2: mprops2.local_mprops.local_com,
-            im1: mprops1.effective_inv_mass,
-            im2: mprops2.effective_inv_mass,
-            ii1,
-            ii2,
-            inv_ii1_ii2,
-            local_anchor1: cparams.local_anchor1,
-            local_anchor2: cparams.local_anchor2,
-            position1: ids1.active_set_offset,
-            position2: ids2.active_set_offset,
-            limits_enabled: cparams.limits_enabled,
-            limits_angle: cparams.limits_angle,
-            limits_local_axis1: cparams.limits_local_axis1,
-            limits_local_axis2: cparams.limits_local_axis2,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position1 = positions[self.position1 as usize];
-        let mut position2 = positions[self.position2 as usize];
-
-        let anchor1 = position1 * self.local_anchor1;
-        let anchor2 = position2 * self.local_anchor2;
-
-        let com1 = position1 * self.local_com1;
-        let com2 = position2 * self.local_com2;
-
-        let err = anchor1 - anchor2;
-
-        let centered_anchor1 = anchor1 - com1;
-        let centered_anchor2 = anchor2 - com2;
-
-        let cmat1 = centered_anchor1.gcross_matrix();
-        let cmat2 = centered_anchor2.gcross_matrix();
-
-        // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose()
-        // because it is anti-symmetric.
-        #[cfg(feature = "dim3")]
-        let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1)
-            + self.ii2.quadform(&cmat2).add_diagonal(self.im2);
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way. It is also
-        // faster because in 2D lhs will be symmetric.
-        #[cfg(feature = "dim2")]
-        let lhs = {
-            let m11 =
-                self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2;
-            let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2;
-            let m22 =
-                self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2;
-            SdpMatrix::new(m11, m12, m22)
-        };
-
-        let inv_lhs = lhs.inverse_unchecked();
-        let impulse = inv_lhs * -(err * params.joint_erp);
-
-        position1.translation.vector += self.im1 * impulse;
-        position2.translation.vector -= self.im2 * impulse;
-
-        let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse));
-        let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
-
-        position1.rotation = Rotation::new(angle1) * position1.rotation;
-        position2.rotation = Rotation::new(angle2) * position2.rotation;
-
-        /*
-         * Limits part.
-         */
-        if self.limits_enabled {
-            let axis1 = position1 * self.limits_local_axis1;
-            let axis2 = position2 * self.limits_local_axis2;
-
-            #[cfg(feature = "dim2")]
-            let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle();
-            #[cfg(feature = "dim3")]
-            let axis_angle =
-                Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle());
-
-            // TODO: handle the case where dot(axis1, axis2) = -1.0
-            if let Some((axis, angle)) = axis_angle {
-                if angle >= self.limits_angle {
-                    #[cfg(feature = "dim2")]
-                    let axis = axis[0];
-                    #[cfg(feature = "dim3")]
-                    let axis = axis.into_inner();
-                    let ang_error = angle - self.limits_angle;
-                    let ang_impulse = self
-                        .inv_ii1_ii2
-                        .transform_vector(axis * ang_error * params.joint_erp);
-
-                    position1.rotation =
-                        Rotation::new(self.ii1.transform_vector(-ang_impulse)) * position1.rotation;
-                    position2.rotation =
-                        Rotation::new(self.ii2.transform_vector(ang_impulse)) * position2.rotation;
-                }
-            }
-        }
-
-        positions[self.position1 as usize] = position1;
-        positions[self.position2 as usize] = position2;
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct BallPositionGroundConstraint {
-    position2: usize,
-    anchor1: Point<Real>,
-    im2: Real,
-    ii2: AngularInertia<Real>,
-    local_anchor2: Point<Real>,
-    local_com2: Point<Real>,
-
-    limits_enabled: bool,
-    limits_angle: Real,
-    limits_axis1: UnitVector<Real>,
-    limits_local_axis2: UnitVector<Real>,
-}
-
-impl BallPositionGroundConstraint {
-    pub fn from_params(
-        rb1: &RigidBodyPosition,
-        rb2: (&RigidBodyMassProps, &RigidBodyIds),
-        cparams: &BallJoint,
-        flipped: bool,
-    ) -> Self {
-        let poss1 = rb1;
-        let (mprops2, ids2) = rb2;
-
-        if flipped {
-            // Note the only thing that is flipped here
-            // are the local_anchors. The rb1 and rb2 have
-            // already been flipped by the caller.
-            Self {
-                anchor1: poss1.next_position * cparams.local_anchor2,
-                im2: mprops2.effective_inv_mass,
-                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
-                local_anchor2: cparams.local_anchor1,
-                position2: ids2.active_set_offset,
-                local_com2: mprops2.local_mprops.local_com,
-                limits_enabled: cparams.limits_enabled,
-                limits_angle: cparams.limits_angle,
-                limits_axis1: poss1.next_position * cparams.limits_local_axis2,
-                limits_local_axis2: cparams.limits_local_axis1,
-            }
-        } else {
-            Self {
-                anchor1: poss1.next_position * cparams.local_anchor1,
-                im2: mprops2.effective_inv_mass,
-                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
-                local_anchor2: cparams.local_anchor2,
-                position2: ids2.active_set_offset,
-                local_com2: mprops2.local_mprops.local_com,
-                limits_enabled: cparams.limits_enabled,
-                limits_angle: cparams.limits_angle,
-                limits_axis1: poss1.next_position * cparams.limits_local_axis1,
-                limits_local_axis2: cparams.limits_local_axis2,
-            }
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position2 = positions[self.position2 as usize];
-
-        let anchor2 = position2 * self.local_anchor2;
-        let com2 = position2 * self.local_com2;
-
-        let err = self.anchor1 - anchor2;
-        let centered_anchor2 = anchor2 - com2;
-        let cmat2 = centered_anchor2.gcross_matrix();
-
-        #[cfg(feature = "dim3")]
-        let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2);
-
-        #[cfg(feature = "dim2")]
-        let lhs = {
-            let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2;
-            let m12 = cmat2.x * cmat2.y * self.ii2;
-            let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2;
-            SdpMatrix::new(m11, m12, m22)
-        };
-
-        let inv_lhs = lhs.inverse_unchecked();
-        let impulse = inv_lhs * -(err * params.joint_erp);
-        position2.translation.vector -= self.im2 * impulse;
-
-        let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
-        position2.rotation = Rotation::new(angle2) * position2.rotation;
-
-        /*
-         * Limits part.
-         */
-        if self.limits_enabled {
-            let axis2 = position2 * self.limits_local_axis2;
-
-            #[cfg(feature = "dim2")]
-            let axis_angle =
-                Rotation::rotation_between_axis(&axis2, &self.limits_axis1).axis_angle();
-            #[cfg(feature = "dim3")]
-            let axis_angle = Rotation::rotation_between_axis(&axis2, &self.limits_axis1)
-                .and_then(|r| r.axis_angle());
-
-            // TODO: handle the case where dot(axis1, axis2) = -1.0
-            if let Some((axis, angle)) = axis_angle {
-                if angle >= self.limits_angle {
-                    #[cfg(feature = "dim2")]
-                    let axis = axis[0];
-                    #[cfg(feature = "dim3")]
-                    let axis = axis.into_inner();
-                    let ang_error = angle - self.limits_angle;
-                    let ang_correction = axis * ang_error * params.joint_erp;
-                    position2.rotation = Rotation::new(ang_correction) * position2.rotation;
-                }
-            }
-        }
-
-        positions[self.position2 as usize] = position2;
-    }
-}

src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs

@@ -1,216 +0,0 @@
-use crate::dynamics::{
-    BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-};
-#[cfg(feature = "dim2")]
-use crate::math::SdpMatrix;
-use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-use simba::simd::SimdValue;
-
-#[derive(Debug)]
-pub(crate) struct WBallPositionConstraint {
-    position1: [usize; SIMD_WIDTH],
-    position2: [usize; SIMD_WIDTH],
-
-    local_com1: Point<SimdReal>,
-    local_com2: Point<SimdReal>,
-
-    im1: SimdReal,
-    im2: SimdReal,
-
-    ii1: AngularInertia<SimdReal>,
-    ii2: AngularInertia<SimdReal>,
-
-    local_anchor1: Point<SimdReal>,
-    local_anchor2: Point<SimdReal>,
-}
-
-impl WBallPositionConstraint {
-    pub fn from_params(
-        rbs1: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&BallJoint; SIMD_WIDTH],
-    ) -> Self {
-        let (mprops1, ids1) = rbs1;
-        let (mprops2, ids2) = rbs2;
-
-        let local_com1 = Point::from(gather![|ii| mprops1[ii].local_mprops.local_com]);
-        let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]);
-        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii1 = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops1[ii].effective_world_inv_inertia_sqrt
-        ])
-        .squared();
-        let ii2 = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ])
-        .squared();
-        let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
-        let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
-        let position1 = gather![|ii| ids1[ii].active_set_offset];
-        let position2 = gather![|ii| ids2[ii].active_set_offset];
-
-        Self {
-            local_com1,
-            local_com2,
-            im1,
-            im2,
-            ii1,
-            ii2,
-            local_anchor1,
-            local_anchor2,
-            position1,
-            position2,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]);
-        let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
-
-        let anchor1 = position1 * self.local_anchor1;
-        let anchor2 = position2 * self.local_anchor2;
-
-        let com1 = position1 * self.local_com1;
-        let com2 = position2 * self.local_com2;
-
-        let err = anchor1 - anchor2;
-
-        let centered_anchor1 = anchor1 - com1;
-        let centered_anchor2 = anchor2 - com2;
-
-        let cmat1 = centered_anchor1.gcross_matrix();
-        let cmat2 = centered_anchor2.gcross_matrix();
-
-        // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose()
-        // because it is anti-symmetric.
-        #[cfg(feature = "dim3")]
-        let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1)
-            + self.ii2.quadform(&cmat2).add_diagonal(self.im2);
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        let lhs = {
-            let m11 =
-                self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2;
-            let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2;
-            let m22 =
-                self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2;
-            SdpMatrix::new(m11, m12, m22)
-        };
-
-        let inv_lhs = lhs.inverse_unchecked();
-        let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
-
-        position1.translation.vector += impulse * self.im1;
-        position2.translation.vector -= impulse * self.im2;
-
-        let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse));
-        let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
-
-        position1.rotation = Rotation::new(angle1) * position1.rotation;
-        position2.rotation = Rotation::new(angle2) * position2.rotation;
-
-        for ii in 0..SIMD_WIDTH {
-            positions[self.position1[ii]] = position1.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            positions[self.position2[ii]] = position2.extract(ii);
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WBallPositionGroundConstraint {
-    position2: [usize; SIMD_WIDTH],
-    anchor1: Point<SimdReal>,
-    im2: SimdReal,
-    ii2: AngularInertia<SimdReal>,
-    local_anchor2: Point<SimdReal>,
-    local_com2: Point<SimdReal>,
-}
-
-impl WBallPositionGroundConstraint {
-    pub fn from_params(
-        rbs1: [&RigidBodyPosition; SIMD_WIDTH],
-        rbs2: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&BallJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        let poss1 = rbs1;
-        let (mprops2, ids2) = rbs2;
-
-        let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]);
-        let anchor1 = position1
-            * Point::from(gather![|ii| if flipped[ii] {
-                cparams[ii].local_anchor2
-            } else {
-                cparams[ii].local_anchor1
-            }]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii2 = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ])
-        .squared();
-        let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
-            cparams[ii].local_anchor1
-        } else {
-            cparams[ii].local_anchor2
-        }]);
-        let position2 = gather![|ii| ids2[ii].active_set_offset];
-        let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]);
-
-        Self {
-            anchor1,
-            im2,
-            ii2,
-            local_anchor2,
-            position2,
-            local_com2,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
-
-        let anchor2 = position2 * self.local_anchor2;
-        let com2 = position2 * self.local_com2;
-
-        let err = self.anchor1 - anchor2;
-        let centered_anchor2 = anchor2 - com2;
-        let cmat2 = centered_anchor2.gcross_matrix();
-
-        #[cfg(feature = "dim3")]
-        let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2);
-
-        #[cfg(feature = "dim2")]
-        let lhs = {
-            let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2;
-            let m12 = cmat2.x * cmat2.y * self.ii2;
-            let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2;
-            SdpMatrix::new(m11, m12, m22)
-        };
-
-        let inv_lhs = lhs.inverse_unchecked();
-        let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
-        position2.translation.vector -= impulse * self.im2;
-
-        let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
-        position2.rotation = Rotation::new(angle2) * position2.rotation;
-
-        for ii in 0..SIMD_WIDTH {
-            positions[self.position2[ii]] = position2.extract(ii);
-        }
-    }
-}

src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs

@@ -1,660 +0,0 @@
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-};
-use crate::math::{AngVector, AngularInertia, Real, Rotation, SdpMatrix, Vector};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
-
-#[derive(Debug)]
-pub(crate) struct BallVelocityConstraint {
-    mj_lambda1: usize,
-    mj_lambda2: usize,
-
-    joint_id: JointIndex,
-
-    rhs: Vector<Real>,
-    impulse: Vector<Real>,
-
-    r1: Vector<Real>,
-    r2: Vector<Real>,
-
-    inv_lhs: SdpMatrix<Real>,
-
-    motor_rhs: AngVector<Real>,
-    motor_impulse: AngVector<Real>,
-    motor_inv_lhs: Option<AngularInertia<Real>>,
-    motor_max_impulse: Real,
-
-    limits_active: bool,
-    limits_rhs: Real,
-    limits_inv_lhs: Real,
-    limits_impulse: Real,
-    limits_axis: AngVector<Real>,
-
-    im1: Real,
-    im2: Real,
-
-    ii1_sqrt: AngularInertia<Real>,
-    ii2_sqrt: AngularInertia<Real>,
-}
-
-impl BallVelocityConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        rb2: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        joint: &BallJoint,
-    ) -> Self {
-        let (rb_pos1, rb_vels1, rb_mprops1, rb_ids1) = rb1;
-        let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2;
-
-        let anchor_world1 = rb_pos1.position * joint.local_anchor1;
-        let anchor_world2 = rb_pos2.position * joint.local_anchor2;
-        let anchor1 = anchor_world1 - rb_mprops1.world_com;
-        let anchor2 = anchor_world2 - rb_mprops2.world_com;
-
-        let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1);
-        let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2);
-        let im1 = rb_mprops1.effective_inv_mass;
-        let im2 = rb_mprops2.effective_inv_mass;
-
-        let rhs = (vel2 - vel1) * params.velocity_solve_fraction
-            + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
-
-        let lhs;
-        let cmat1 = anchor1.gcross_matrix();
-        let cmat2 = anchor2.gcross_matrix();
-
-        #[cfg(feature = "dim3")]
-        {
-            lhs = rb_mprops2
-                .effective_world_inv_inertia_sqrt
-                .squared()
-                .quadform(&cmat2)
-                .add_diagonal(im2)
-                + rb_mprops1
-                    .effective_world_inv_inertia_sqrt
-                    .squared()
-                    .quadform(&cmat1)
-                    .add_diagonal(im1);
-        }
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        {
-            let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
-            let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
-            let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
-            let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
-            let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
-            lhs = SdpMatrix::new(m11, m12, m22)
-        }
-
-        let inv_lhs = lhs.inverse_unchecked();
-
-        /*
-         * Motor part.
-         */
-        let mut motor_rhs = na::zero();
-        let mut motor_inv_lhs = None;
-        let motor_max_impulse = joint.motor_max_impulse;
-
-        if motor_max_impulse > 0.0 {
-            let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
-                params.dt,
-                joint.motor_stiffness,
-                joint.motor_damping,
-            );
-
-            if stiffness != 0.0 {
-                let dpos = rb_pos2.position.rotation
-                    * (rb_pos1.position.rotation * joint.motor_target_pos).inverse();
-                #[cfg(feature = "dim2")]
-                {
-                    motor_rhs += dpos.angle() * stiffness;
-                }
-                #[cfg(feature = "dim3")]
-                {
-                    motor_rhs += dpos.scaled_axis() * stiffness;
-                }
-            }
-
-            if damping != 0.0 {
-                let curr_vel = rb_vels2.angvel - rb_vels1.angvel;
-                motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
-            }
-
-            #[cfg(feature = "dim2")]
-            if stiffness != 0.0 || damping != 0.0 {
-                motor_inv_lhs = if keep_lhs {
-                    let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
-                    let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
-                    Some(gamma / (ii1 + ii2))
-                } else {
-                    Some(gamma)
-                };
-                motor_rhs /= gamma;
-            }
-
-            #[cfg(feature = "dim3")]
-            if stiffness != 0.0 || damping != 0.0 {
-                motor_inv_lhs = if keep_lhs {
-                    let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
-                    let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
-                    Some((ii1 + ii2).inverse_unchecked() * gamma)
-                } else {
-                    Some(SdpMatrix::diagonal(gamma))
-                };
-                motor_rhs /= gamma;
-            }
-        }
-
-        #[cfg(feature = "dim2")]
-        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
-            * params.warmstart_coeff;
-        #[cfg(feature = "dim3")]
-        let motor_impulse =
-            joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
-
-        /*
-         * Setup the limits constraint.
-         */
-        let mut limits_active = false;
-        let mut limits_rhs = 0.0;
-        let mut limits_inv_lhs = 0.0;
-        let mut limits_impulse = 0.0;
-        let mut limits_axis = na::zero();
-
-        if joint.limits_enabled {
-            let axis1 = rb_pos1.position * joint.limits_local_axis1;
-            let axis2 = rb_pos2.position * joint.limits_local_axis2;
-
-            #[cfg(feature = "dim2")]
-            let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle();
-            #[cfg(feature = "dim3")]
-            let axis_angle =
-                Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle());
-
-            // TODO: handle the case where dot(axis1, axis2) = -1.0
-            if let Some((axis, angle)) = axis_angle {
-                if angle >= joint.limits_angle {
-                    #[cfg(feature = "dim2")]
-                    let axis = axis[0];
-                    #[cfg(feature = "dim3")]
-                    let axis = axis.into_inner();
-
-                    limits_active = true;
-                    limits_rhs = (rb_vels2.angvel.gdot(axis) - rb_vels1.angvel.gdot(axis))
-                        * params.velocity_solve_fraction;
-
-                    limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt();
-
-                    let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
-                    let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
-                    limits_inv_lhs = crate::utils::inv(
-                        axis.gdot(ii2.transform_vector(axis))
-                            + axis.gdot(ii1.transform_vector(axis)),
-                    );
-
-                    limits_impulse = joint.limits_impulse * params.warmstart_coeff;
-                    limits_axis = axis;
-                }
-            }
-        }
-
-        BallVelocityConstraint {
-            joint_id,
-            mj_lambda1: rb_ids1.active_set_offset,
-            mj_lambda2: rb_ids2.active_set_offset,
-            im1,
-            im2,
-            impulse: joint.impulse * params.warmstart_coeff,
-            r1: anchor1,
-            r2: anchor2,
-            rhs,
-            inv_lhs,
-            motor_rhs,
-            motor_impulse,
-            motor_inv_lhs,
-            motor_max_impulse: joint.motor_max_impulse,
-            ii1_sqrt: rb_mprops1.effective_world_inv_inertia_sqrt,
-            ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt,
-            limits_active,
-            limits_axis,
-            limits_rhs,
-            limits_inv_lhs,
-            limits_impulse,
-        }
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        mj_lambda1.linear += self.im1 * self.impulse;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(self.r1.gcross(self.impulse) + self.motor_impulse);
-        mj_lambda2.linear -= self.im2 * self.impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse);
-
-        /*
-         * Warmstart limits.
-         */
-        if self.limits_active {
-            let limit_impulse1 = -self.limits_axis * self.limits_impulse;
-            let limit_impulse2 = self.limits_axis * self.limits_impulse;
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(limit_impulse1);
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
-        }
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
-        let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let dvel = -vel1 + vel2 + self.rhs;
-
-        let impulse = self.inv_lhs * dvel;
-        self.impulse += impulse;
-
-        mj_lambda1.linear += self.im1 * impulse;
-        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(impulse));
-
-        mj_lambda2.linear -= self.im2 * impulse;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
-    }
-
-    fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.limits_active {
-            let limits_torquedir1 = -self.limits_axis;
-            let limits_torquedir2 = self.limits_axis;
-
-            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let ang_dvel = limits_torquedir1.gdot(ang_vel1)
-                + limits_torquedir2.gdot(ang_vel2)
-                + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0);
-            let dimpulse = new_impulse - self.limits_impulse;
-            self.limits_impulse = new_impulse;
-
-            let ang_impulse1 = limits_torquedir1 * dimpulse;
-            let ang_impulse2 = limits_torquedir2 * dimpulse;
-
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(ang_impulse1);
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
-        }
-    }
-
-    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
-            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs;
-
-            let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
-
-            #[cfg(feature = "dim2")]
-            let clamped_impulse =
-                na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse);
-            #[cfg(feature = "dim3")]
-            let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
-
-            let effective_impulse = clamped_impulse - self.motor_impulse;
-            self.motor_impulse = clamped_impulse;
-
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse);
-            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
-        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
-        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        if let JointParams::BallJoint(ball) = &mut joint.params {
-            ball.impulse = self.impulse;
-            ball.motor_impulse = self.motor_impulse;
-            ball.limits_impulse = self.limits_impulse;
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct BallVelocityGroundConstraint {
-    mj_lambda2: usize,
-    joint_id: JointIndex,
-    r2: Vector<Real>,
-
-    rhs: Vector<Real>,
-    impulse: Vector<Real>,
-    inv_lhs: SdpMatrix<Real>,
-
-    motor_rhs: AngVector<Real>,
-    motor_impulse: AngVector<Real>,
-    motor_inv_lhs: Option<AngularInertia<Real>>,
-    motor_max_impulse: Real,
-
-    limits_active: bool,
-    limits_rhs: Real,
-    limits_inv_lhs: Real,
-    limits_impulse: Real,
-    limits_axis: AngVector<Real>,
-
-    im2: Real,
-    ii2_sqrt: AngularInertia<Real>,
-}
-
-impl BallVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
-        rb2: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        joint: &BallJoint,
-        flipped: bool,
-    ) -> Self {
-        let (rb_pos1, rb_vels1, rb_mprops1) = rb1;
-        let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2;
-
-        let (anchor_world1, anchor_world2) = if flipped {
-            (
-                rb_pos1.position * joint.local_anchor2,
-                rb_pos2.position * joint.local_anchor1,
-            )
-        } else {
-            (
-                rb_pos1.position * joint.local_anchor1,
-                rb_pos2.position * joint.local_anchor2,
-            )
-        };
-
-        let anchor1 = anchor_world1 - rb_mprops1.world_com;
-        let anchor2 = anchor_world2 - rb_mprops2.world_com;
-
-        let im2 = rb_mprops2.effective_inv_mass;
-        let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1);
-        let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2);
-
-        let rhs = (vel2 - vel1) * params.velocity_solve_fraction
-            + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
-
-        let cmat2 = anchor2.gcross_matrix();
-
-        let lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            lhs = rb_mprops2
-                .effective_world_inv_inertia_sqrt
-                .squared()
-                .quadform(&cmat2)
-                .add_diagonal(im2);
-        }
-
-        #[cfg(feature = "dim2")]
-        {
-            let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
-            let m11 = im2 + cmat2.x * cmat2.x * ii2;
-            let m12 = cmat2.x * cmat2.y * ii2;
-            let m22 = im2 + cmat2.y * cmat2.y * ii2;
-            lhs = SdpMatrix::new(m11, m12, m22)
-        }
-
-        let inv_lhs = lhs.inverse_unchecked();
-
-        /*
-         * Motor part.
-         */
-        let mut motor_rhs = na::zero();
-        let mut motor_inv_lhs = None;
-        let motor_max_impulse = joint.motor_max_impulse;
-
-        if motor_max_impulse > 0.0 {
-            let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
-                params.dt,
-                joint.motor_stiffness,
-                joint.motor_damping,
-            );
-
-            if stiffness != 0.0 {
-                let dpos = rb_pos2.position.rotation
-                    * (rb_pos1.position.rotation * joint.motor_target_pos).inverse();
-                #[cfg(feature = "dim2")]
-                {
-                    motor_rhs += dpos.angle() * stiffness;
-                }
-                #[cfg(feature = "dim3")]
-                {
-                    motor_rhs += dpos.scaled_axis() * stiffness;
-                }
-            }
-
-            if damping != 0.0 {
-                let curr_vel = rb_vels2.angvel - rb_vels1.angvel;
-                motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
-            }
-
-            #[cfg(feature = "dim2")]
-            if stiffness != 0.0 || damping != 0.0 {
-                motor_inv_lhs = if keep_lhs {
-                    let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
-                    Some(gamma / ii2)
-                } else {
-                    Some(gamma)
-                };
-                motor_rhs /= gamma;
-            }
-
-            #[cfg(feature = "dim3")]
-            if stiffness != 0.0 || damping != 0.0 {
-                motor_inv_lhs = if keep_lhs {
-                    let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
-                    Some(ii2.inverse_unchecked() * gamma)
-                } else {
-                    Some(SdpMatrix::diagonal(gamma))
-                };
-                motor_rhs /= gamma;
-            }
-        }
-
-        #[cfg(feature = "dim2")]
-        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
-            * params.warmstart_coeff;
-        #[cfg(feature = "dim3")]
-        let motor_impulse =
-            joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
-
-        /*
-         * Setup the limits constraint.
-         */
-        let mut limits_active = false;
-        let mut limits_rhs = 0.0;
-        let mut limits_inv_lhs = 0.0;
-        let mut limits_impulse = 0.0;
-        let mut limits_axis = na::zero();
-
-        if joint.limits_enabled {
-            let (axis1, axis2) = if flipped {
-                (
-                    rb_pos1.position * joint.limits_local_axis2,
-                    rb_pos2.position * joint.limits_local_axis1,
-                )
-            } else {
-                (
-                    rb_pos1.position * joint.limits_local_axis1,
-                    rb_pos2.position * joint.limits_local_axis2,
-                )
-            };
-
-            #[cfg(feature = "dim2")]
-            let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle();
-            #[cfg(feature = "dim3")]
-            let axis_angle =
-                Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle());
-
-            // TODO: handle the case where dot(axis1, axis2) = -1.0
-            if let Some((axis, angle)) = axis_angle {
-                if angle >= joint.limits_angle {
-                    #[cfg(feature = "dim2")]
-                    let axis = axis[0];
-                    #[cfg(feature = "dim3")]
-                    let axis = axis.into_inner();
-
-                    limits_active = true;
-                    limits_rhs = (rb_vels2.angvel.gdot(axis) - rb_vels1.angvel.gdot(axis))
-                        * params.velocity_solve_fraction;
-
-                    limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt();
-
-                    let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
-                    limits_inv_lhs = crate::utils::inv(axis.gdot(ii2.transform_vector(axis)));
-                    limits_impulse = joint.limits_impulse * params.warmstart_coeff;
-                    limits_axis = axis;
-                }
-            }
-        }
-
-        BallVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2: rb_ids2.active_set_offset,
-            im2,
-            impulse: joint.impulse * params.warmstart_coeff,
-            r2: anchor2,
-            rhs,
-            inv_lhs,
-            motor_rhs,
-            motor_impulse,
-            motor_inv_lhs,
-            motor_max_impulse: joint.motor_max_impulse,
-            ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt,
-            limits_active,
-            limits_axis,
-            limits_rhs,
-            limits_inv_lhs,
-            limits_impulse,
-        }
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-        mj_lambda2.linear -= self.im2 * self.impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse);
-
-        /*
-         * Warmstart limits.
-         */
-        if self.limits_active {
-            let limit_impulse2 = self.limits_axis * self.limits_impulse;
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
-        }
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
-        let dvel = vel2 + self.rhs;
-
-        let impulse = self.inv_lhs * dvel;
-        self.impulse += impulse;
-
-        mj_lambda2.linear -= self.im2 * impulse;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
-    }
-
-    fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.limits_active {
-            let limits_torquedir2 = self.limits_axis;
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let ang_dvel = limits_torquedir2.gdot(ang_vel2) + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0);
-            let dimpulse = new_impulse - self.limits_impulse;
-            self.limits_impulse = new_impulse;
-
-            let ang_impulse2 = limits_torquedir2 * dimpulse;
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
-        }
-    }
-
-    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let dangvel = ang_vel2 + self.motor_rhs;
-            let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
-
-            #[cfg(feature = "dim2")]
-            let clamped_impulse =
-                na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse);
-            #[cfg(feature = "dim3")]
-            let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
-
-            let effective_impulse = clamped_impulse - self.motor_impulse;
-            self.motor_impulse = clamped_impulse;
-
-            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        self.solve_limits(&mut mj_lambda2);
-        self.solve_dofs(&mut mj_lambda2);
-        self.solve_motors(&mut mj_lambda2);
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    // FIXME: duplicated code with the non-ground constraint.
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        if let JointParams::BallJoint(ball) = &mut joint.params {
-            ball.impulse = self.impulse;
-            ball.motor_impulse = self.motor_impulse;
-            ball.limits_impulse = self.limits_impulse;
-        }
-    }
-}

src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs

@@ -1,359 +0,0 @@
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-};
-use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
-};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-use simba::simd::SimdValue;
-
-#[derive(Debug)]
-pub(crate) struct WBallVelocityConstraint {
-    mj_lambda1: [usize; SIMD_WIDTH],
-    mj_lambda2: [usize; SIMD_WIDTH],
-
-    joint_id: [JointIndex; SIMD_WIDTH],
-
-    rhs: Vector<SimdReal>,
-    pub(crate) impulse: Vector<SimdReal>,
-
-    r1: Vector<SimdReal>,
-    r2: Vector<SimdReal>,
-
-    inv_lhs: SdpMatrix<SimdReal>,
-
-    im1: SimdReal,
-    im2: SimdReal,
-
-    ii1_sqrt: AngularInertia<SimdReal>,
-    ii2_sqrt: AngularInertia<SimdReal>,
-}
-
-impl WBallVelocityConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&BallJoint; SIMD_WIDTH],
-    ) -> Self {
-        let (poss1, vels1, mprops1, ids1) = rbs1;
-        let (poss2, vels2, mprops2, ids2) = rbs2;
-
-        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
-        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
-        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
-        let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops1[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
-
-        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
-        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
-
-        let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
-        let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
-        let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
-
-        let anchor_world1 = position1 * local_anchor1;
-        let anchor_world2 = position2 * local_anchor2;
-        let anchor1 = anchor_world1 - world_com1;
-        let anchor2 = anchor_world2 - world_com2;
-
-        let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
-        let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
-        let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction)
-            + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt());
-        let lhs;
-
-        let cmat1 = anchor1.gcross_matrix();
-        let cmat2 = anchor2.gcross_matrix();
-
-        #[cfg(feature = "dim3")]
-        {
-            lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2)
-                + ii1_sqrt.squared().quadform(&cmat1).add_diagonal(im1);
-        }
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        {
-            let ii1 = ii1_sqrt.squared();
-            let ii2 = ii2_sqrt.squared();
-            let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
-            let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
-            let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
-            lhs = SdpMatrix::new(m11, m12, m22)
-        }
-
-        let inv_lhs = lhs.inverse_unchecked();
-
-        WBallVelocityConstraint {
-            joint_id,
-            mj_lambda1,
-            mj_lambda2,
-            im1,
-            im2,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            r1: anchor1,
-            r2: anchor2,
-            rhs,
-            inv_lhs,
-            ii1_sqrt,
-            ii2_sqrt,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        mj_lambda1.linear += self.impulse * self.im1;
-        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse));
-        mj_lambda2.linear -= self.impulse * self.im2;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
-        let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let dvel = -vel1 + vel2 + self.rhs;
-
-        let impulse = self.inv_lhs * dvel;
-        self.impulse += impulse;
-
-        mj_lambda1.linear += impulse * self.im1;
-        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(impulse));
-
-        mj_lambda2.linear -= impulse * self.im2;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::BallJoint(ball) = &mut joint.params {
-                ball.impulse = self.impulse.extract(ii)
-            }
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WBallVelocityGroundConstraint {
-    mj_lambda2: [usize; SIMD_WIDTH],
-    joint_id: [JointIndex; SIMD_WIDTH],
-    rhs: Vector<SimdReal>,
-    pub(crate) impulse: Vector<SimdReal>,
-    r2: Vector<SimdReal>,
-    inv_lhs: SdpMatrix<SimdReal>,
-    im2: SimdReal,
-    ii2_sqrt: AngularInertia<SimdReal>,
-}
-
-impl WBallVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&BallJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        let (poss1, vels1, mprops1) = rbs1;
-        let (poss2, vels2, mprops2, ids2) = rbs2;
-
-        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
-        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
-        let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
-            cparams[ii].local_anchor2
-        } else {
-            cparams[ii].local_anchor1
-        }]);
-
-        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
-        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
-
-        let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
-            cparams[ii].local_anchor1
-        } else {
-            cparams[ii].local_anchor2
-        }]);
-        let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
-
-        let anchor_world1 = position1 * local_anchor1;
-        let anchor_world2 = position2 * local_anchor2;
-        let anchor1 = anchor_world1 - world_com1;
-        let anchor2 = anchor_world2 - world_com2;
-
-        let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
-        let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
-        let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction)
-            + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt());
-        let lhs;
-
-        let cmat2 = anchor2.gcross_matrix();
-
-        #[cfg(feature = "dim3")]
-        {
-            lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2);
-        }
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        {
-            let ii2 = ii2_sqrt.squared();
-            let m11 = im2 + cmat2.x * cmat2.x * ii2;
-            let m12 = cmat2.x * cmat2.y * ii2;
-            let m22 = im2 + cmat2.y * cmat2.y * ii2;
-            lhs = SdpMatrix::new(m11, m12, m22)
-        }
-
-        let inv_lhs = lhs.inverse_unchecked();
-
-        WBallVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2,
-            im2,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            r2: anchor2,
-            rhs,
-            inv_lhs,
-            ii2_sqrt,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        mj_lambda2.linear -= self.impulse * self.im2;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
-        let dvel = vel2 + self.rhs;
-
-        let impulse = self.inv_lhs * dvel;
-        self.impulse += impulse;
-
-        mj_lambda2.linear -= impulse * self.im2;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::BallJoint(ball) = &mut joint.params {
-                ball.impulse = self.impulse.extract(ii)
-            }
-        }
-    }
-}

src/dynamics/solver/joint_constraint/fixed_position_constraint.rs

@@ -1,151 +0,0 @@
-use crate::dynamics::{
-    FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-};
-use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
-use crate::utils::WAngularInertia;
-
-#[derive(Debug)]
-pub(crate) struct FixedPositionConstraint {
-    position1: usize,
-    position2: usize,
-    local_frame1: Isometry<Real>,
-    local_frame2: Isometry<Real>,
-    local_com1: Point<Real>,
-    local_com2: Point<Real>,
-    im1: Real,
-    im2: Real,
-    ii1: AngularInertia<Real>,
-    ii2: AngularInertia<Real>,
-
-    lin_inv_lhs: Real,
-    ang_inv_lhs: AngularInertia<Real>,
-}
-
-impl FixedPositionConstraint {
-    pub fn from_params(
-        rb1: (&RigidBodyMassProps, &RigidBodyIds),
-        rb2: (&RigidBodyMassProps, &RigidBodyIds),
-        cparams: &FixedJoint,
-    ) -> Self {
-        let (mprops1, ids1) = rb1;
-        let (mprops2, ids2) = rb2;
-
-        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let im1 = mprops1.effective_inv_mass;
-        let im2 = mprops2.effective_inv_mass;
-        let lin_inv_lhs = 1.0 / (im1 + im2);
-        let ang_inv_lhs = (ii1 + ii2).inverse();
-
-        Self {
-            local_frame1: cparams.local_frame1,
-            local_frame2: cparams.local_frame2,
-            position1: ids1.active_set_offset,
-            position2: ids2.active_set_offset,
-            im1,
-            im2,
-            ii1,
-            ii2,
-            local_com1: mprops1.local_mprops.local_com,
-            local_com2: mprops2.local_mprops.local_com,
-            lin_inv_lhs,
-            ang_inv_lhs,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position1 = positions[self.position1 as usize];
-        let mut position2 = positions[self.position2 as usize];
-
-        // Angular correction.
-        let anchor1 = position1 * self.local_frame1;
-        let anchor2 = position2 * self.local_frame2;
-        let ang_err = anchor2.rotation * anchor1.rotation.inverse();
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self
-            .ang_inv_lhs
-            .transform_vector(ang_err.scaled_axis() * params.joint_erp);
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self
-            .ang_inv_lhs
-            .transform_vector(ang_err.angle() * params.joint_erp);
-        position1.rotation =
-            Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
-        position2.rotation =
-            Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
-
-        // Linear correction.
-        let anchor1 = position1 * Point::from(self.local_frame1.translation.vector);
-        let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
-        let err = anchor2 - anchor1;
-        let impulse = err * (self.lin_inv_lhs * params.joint_erp);
-        position1.translation.vector += self.im1 * impulse;
-        position2.translation.vector -= self.im2 * impulse;
-
-        positions[self.position1 as usize] = position1;
-        positions[self.position2 as usize] = position2;
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct FixedPositionGroundConstraint {
-    position2: usize,
-    anchor1: Isometry<Real>,
-    local_frame2: Isometry<Real>,
-    local_com2: Point<Real>,
-    im2: Real,
-    ii2: AngularInertia<Real>,
-    impulse: Real,
-}
-
-impl FixedPositionGroundConstraint {
-    pub fn from_params(
-        rb1: &RigidBodyPosition,
-        rb2: (&RigidBodyMassProps, &RigidBodyIds),
-        cparams: &FixedJoint,
-        flipped: bool,
-    ) -> Self {
-        let poss1 = rb1;
-        let (mprops2, ids2) = rb2;
-
-        let anchor1;
-        let local_frame2;
-
-        if flipped {
-            anchor1 = poss1.next_position * cparams.local_frame2;
-            local_frame2 = cparams.local_frame1;
-        } else {
-            anchor1 = poss1.next_position * cparams.local_frame1;
-            local_frame2 = cparams.local_frame2;
-        };
-
-        Self {
-            anchor1,
-            local_frame2,
-            position2: ids2.active_set_offset,
-            im2: mprops2.effective_inv_mass,
-            ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
-            local_com2: mprops2.local_mprops.local_com,
-            impulse: 0.0,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position2 = positions[self.position2 as usize];
-
-        // Angular correction.
-        let anchor2 = position2 * self.local_frame2;
-        let ang_err = anchor2.rotation * self.anchor1.rotation.inverse();
-        position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
-
-        // Linear correction.
-        let anchor1 = Point::from(self.anchor1.translation.vector);
-        let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
-        let err = anchor2 - anchor1;
-        // NOTE: no need to divide by im2 just to multiply right after.
-        let impulse = err * params.joint_erp;
-        position2.translation.vector -= impulse;
-
-        positions[self.position2 as usize] = position2;
-    }
-}

src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs

@@ -1,71 +0,0 @@
-use super::{FixedPositionConstraint, FixedPositionGroundConstraint};
-use crate::dynamics::{
-    FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-};
-use crate::math::{Isometry, Real, SIMD_WIDTH};
-
-// TODO: this does not uses SIMD optimizations yet.
-#[derive(Debug)]
-pub(crate) struct WFixedPositionConstraint {
-    constraints: [FixedPositionConstraint; SIMD_WIDTH],
-}
-
-impl WFixedPositionConstraint {
-    pub fn from_params(
-        rbs1: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&FixedJoint; SIMD_WIDTH],
-    ) -> Self {
-        Self {
-            constraints: gather![|ii| FixedPositionConstraint::from_params(
-                (rbs1.0[ii], rbs1.1[ii]),
-                (rbs2.0[ii], rbs2.1[ii]),
-                cparams[ii]
-            )],
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        for constraint in &self.constraints {
-            constraint.solve(params, positions);
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WFixedPositionGroundConstraint {
-    constraints: [FixedPositionGroundConstraint; SIMD_WIDTH],
-}
-
-impl WFixedPositionGroundConstraint {
-    pub fn from_params(
-        rbs1: [&RigidBodyPosition; SIMD_WIDTH],
-        rbs2: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&FixedJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        Self {
-            constraints: gather![|ii| FixedPositionGroundConstraint::from_params(
-                rbs1[ii],
-                (rbs2.0[ii], rbs2.1[ii]),
-                cparams[ii],
-                flipped[ii]
-            )],
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        for constraint in &self.constraints {
-            constraint.solve(params, positions);
-        }
-    }
-}

src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs

@@ -1,436 +0,0 @@
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-};
-use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-#[cfg(feature = "dim2")]
-use na::{Matrix3, Vector3};
-#[cfg(feature = "dim3")]
-use na::{Matrix6, Vector6};
-
-#[derive(Debug)]
-pub(crate) struct FixedVelocityConstraint {
-    mj_lambda1: usize,
-    mj_lambda2: usize,
-
-    joint_id: JointIndex,
-
-    impulse: SpacialVector<Real>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
-    #[cfg(feature = "dim3")]
-    rhs: Vector6<Real>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
-    #[cfg(feature = "dim2")]
-    rhs: Vector3<Real>,
-
-    im1: Real,
-    im2: Real,
-
-    ii1: AngularInertia<Real>,
-    ii2: AngularInertia<Real>,
-
-    ii1_sqrt: AngularInertia<Real>,
-    ii2_sqrt: AngularInertia<Real>,
-
-    r1: Vector<Real>,
-    r2: Vector<Real>,
-}
-
-impl FixedVelocityConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        rb2: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        cparams: &FixedJoint,
-    ) -> Self {
-        let (poss1, vels1, mprops1, ids1) = rb1;
-        let (poss2, vels2, mprops2, ids2) = rb2;
-
-        let anchor1 = poss1.position * cparams.local_frame1;
-        let anchor2 = poss2.position * cparams.local_frame2;
-        let im1 = mprops1.effective_inv_mass;
-        let im2 = mprops2.effective_inv_mass;
-        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1.translation.vector - mprops1.world_com.coords;
-        let r2 = anchor2.translation.vector - mprops2.world_com.coords;
-        let rmat1 = r1.gcross_matrix();
-        let rmat2 = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let lhs00 =
-                ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2);
-            let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2);
-            let lhs11 = (ii1 + ii2).into_matrix();
-
-            // Note that Cholesky only reads the lower-triangular part of the matrix
-            // so we don't need to fill lhs01.
-            lhs = Matrix6::zeros();
-            lhs.fixed_slice_mut::<3, 3>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<3, 3>(3, 3).copy_from(&lhs11);
-        }
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        {
-            let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2;
-            let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2;
-            let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2;
-            let m13 = rmat1.x * ii1 + rmat2.x * ii2;
-            let m23 = rmat1.y * ii1 + rmat2.y * ii2;
-            let m33 = ii1 + ii2;
-            lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
-        }
-
-        // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix
-        // for which a textbook inverse is still efficient.
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.try_inverse().expect("Singular system.");
-        #[cfg(feature = "dim3")]
-        let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
-
-        let lin_dvel =
-            -vels1.linvel - vels1.angvel.gcross(r1) + vels2.linvel + vels2.angvel.gcross(r2);
-        let ang_dvel = -vels1.angvel + vels2.angvel;
-
-        #[cfg(feature = "dim2")]
-        let mut rhs =
-            Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction;
-
-        #[cfg(feature = "dim3")]
-        let mut rhs = Vector6::new(
-            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        ) * params.velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let lin_err = anchor2.translation.vector - anchor1.translation.vector;
-            let ang_err = anchor2.rotation * anchor1.rotation.inverse();
-
-            #[cfg(feature = "dim2")]
-            {
-                let ang_err = ang_err.angle();
-                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
-            }
-
-            #[cfg(feature = "dim3")]
-            {
-                let ang_err = ang_err.scaled_axis();
-                rhs += Vector6::new(
-                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
-                ) * velocity_based_erp_inv_dt;
-            }
-        }
-
-        FixedVelocityConstraint {
-            joint_id,
-            mj_lambda1: ids1.active_set_offset,
-            mj_lambda2: ids2.active_set_offset,
-            im1,
-            im2,
-            ii1,
-            ii2,
-            ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
-            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
-            inv_lhs,
-            r1,
-            r2,
-            rhs,
-        }
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<3>(3).into_owned();
-
-        mj_lambda1.linear += self.im1 * lin_impulse;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-        let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
-            + mj_lambda2.linear
-            + ang_vel2.gcross(self.r2);
-        let dangvel = -ang_vel1 + ang_vel2;
-
-        #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
-        ) + self.rhs;
-
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<3>(3).into_owned();
-
-        mj_lambda1.linear += self.im1 * lin_impulse;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        if let JointParams::FixedJoint(fixed) = &mut joint.params {
-            fixed.impulse = self.impulse;
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct FixedVelocityGroundConstraint {
-    mj_lambda2: usize,
-
-    joint_id: JointIndex,
-
-    impulse: SpacialVector<Real>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
-    #[cfg(feature = "dim3")]
-    rhs: Vector6<Real>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
-    #[cfg(feature = "dim2")]
-    rhs: Vector3<Real>,
-
-    im2: Real,
-    ii2: AngularInertia<Real>,
-    ii2_sqrt: AngularInertia<Real>,
-    r2: Vector<Real>,
-}
-
-impl FixedVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
-        rb2: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        cparams: &FixedJoint,
-        flipped: bool,
-    ) -> Self {
-        let (poss1, vels1, mprops1) = rb1;
-        let (poss2, vels2, mprops2, ids2) = rb2;
-
-        let (anchor1, anchor2) = if flipped {
-            (
-                poss1.position * cparams.local_frame2,
-                poss2.position * cparams.local_frame1,
-            )
-        } else {
-            (
-                poss1.position * cparams.local_frame1,
-                poss2.position * cparams.local_frame2,
-            )
-        };
-
-        let r1 = anchor1.translation.vector - mprops1.world_com.coords;
-
-        let im2 = mprops2.effective_inv_mass;
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let r2 = anchor2.translation.vector - mprops2.world_com.coords;
-        let rmat2 = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D.
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2);
-            let lhs10 = ii2.transform_matrix(&rmat2);
-            let lhs11 = ii2.into_matrix();
-
-            // Note that Cholesky only reads the lower-triangular part of the matrix
-            // so we don't need to fill lhs01.
-            lhs = Matrix6::zeros();
-            lhs.fixed_slice_mut::<3, 3>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<3, 3>(3, 3).copy_from(&lhs11);
-        }
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        {
-            let m11 = im2 + rmat2.x * rmat2.x * ii2;
-            let m12 = rmat2.x * rmat2.y * ii2;
-            let m22 = im2 + rmat2.y * rmat2.y * ii2;
-            let m13 = rmat2.x * ii2;
-            let m23 = rmat2.y * ii2;
-            let m33 = ii2;
-            lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
-        }
-
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.try_inverse().expect("Singular system.");
-        #[cfg(feature = "dim3")]
-        let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
-
-        let lin_dvel =
-            vels2.linvel + vels2.angvel.gcross(r2) - vels1.linvel - vels1.angvel.gcross(r1);
-        let ang_dvel = vels2.angvel - vels1.angvel;
-
-        #[cfg(feature = "dim2")]
-        let mut rhs =
-            Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction;
-        #[cfg(feature = "dim3")]
-        let mut rhs = Vector6::new(
-            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        ) * params.velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let lin_err = anchor2.translation.vector - anchor1.translation.vector;
-            let ang_err = anchor2.rotation * anchor1.rotation.inverse();
-
-            #[cfg(feature = "dim2")]
-            {
-                let ang_err = ang_err.angle();
-                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
-            }
-
-            #[cfg(feature = "dim3")]
-            {
-                let ang_err = ang_err.scaled_axis();
-                rhs += Vector6::new(
-                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
-                ) * velocity_based_erp_inv_dt;
-            }
-        }
-
-        FixedVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2: ids2.active_set_offset,
-            im2,
-            ii2,
-            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
-            inv_lhs,
-            r2,
-            rhs,
-        }
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<3>(3).into_owned();
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-        let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let dangvel = ang_vel2;
-        #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
-        ) + self.rhs;
-
-        let impulse = self.inv_lhs * rhs;
-
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<3>(3).into_owned();
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    // FIXME: duplicated code with the non-ground constraint.
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        if let JointParams::FixedJoint(fixed) = &mut joint.params {
-            fixed.impulse = self.impulse;
-        }
-    }
-}

src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs

@@ -1,539 +0,0 @@
-use simba::simd::SimdValue;
-
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-};
-use crate::math::{
-    AngVector, AngularInertia, CrossMatrix, Isometry, Point, Real, SimdReal, SpacialVector, Vector,
-    DIM, SIMD_WIDTH,
-};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-#[cfg(feature = "dim3")]
-use na::{Cholesky, Matrix6, Vector3, Vector6};
-#[cfg(feature = "dim2")]
-use {
-    na::{Matrix3, Vector3},
-    parry::utils::SdpMatrix3,
-};
-
-#[derive(Debug)]
-pub(crate) struct WFixedVelocityConstraint {
-    mj_lambda1: [usize; SIMD_WIDTH],
-    mj_lambda2: [usize; SIMD_WIDTH],
-
-    joint_id: [JointIndex; SIMD_WIDTH],
-
-    impulse: SpacialVector<SimdReal>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
-    #[cfg(feature = "dim3")]
-    rhs: Vector6<SimdReal>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<SimdReal>,
-    #[cfg(feature = "dim2")]
-    rhs: Vector3<SimdReal>,
-
-    im1: SimdReal,
-    im2: SimdReal,
-
-    ii1: AngularInertia<SimdReal>,
-    ii2: AngularInertia<SimdReal>,
-
-    ii1_sqrt: AngularInertia<SimdReal>,
-    ii2_sqrt: AngularInertia<SimdReal>,
-
-    r1: Vector<SimdReal>,
-    r2: Vector<SimdReal>,
-}
-
-impl WFixedVelocityConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&FixedJoint; SIMD_WIDTH],
-    ) -> Self {
-        let (poss1, vels1, mprops1, ids1) = rbs1;
-        let (poss2, vels2, mprops2, ids2) = rbs2;
-
-        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
-        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
-        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
-        let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops1[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
-
-        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
-        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
-
-        let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1]);
-        let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2]);
-        let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
-
-        let anchor1 = position1 * local_frame1;
-        let anchor2 = position2 * local_frame2;
-        let ii1 = ii1_sqrt.squared();
-        let ii2 = ii2_sqrt.squared();
-        let r1 = anchor1.translation.vector - world_com1.coords;
-        let r2 = anchor2.translation.vector - world_com2.coords;
-        let rmat1: CrossMatrix<_> = r1.gcross_matrix();
-        let rmat2: CrossMatrix<_> = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D.
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let lhs00 =
-                ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2);
-            let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2);
-            let lhs11 = (ii1 + ii2).into_matrix();
-
-            // Note that Cholesky only reads the lower-triangular part of the matrix
-            // so we don't need to fill lhs01.
-            lhs = Matrix6::zeros();
-            lhs.fixed_slice_mut::<3, 3>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<3, 3>(3, 3).copy_from(&lhs11);
-        }
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        {
-            let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2;
-            let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2;
-            let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2;
-            let m13 = rmat1.x * ii1 + rmat2.x * ii2;
-            let m23 = rmat1.y * ii1 + rmat2.y * ii2;
-            let m33 = ii1 + ii2;
-            lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
-        }
-
-        // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix
-        // for which a textbook inverse is still efficient.
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't extract the matrix?
-        #[cfg(feature = "dim3")]
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2);
-        let ang_dvel = -angvel1 + angvel2;
-
-        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
-
-        #[cfg(feature = "dim2")]
-        let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction;
-
-        #[cfg(feature = "dim3")]
-        let mut rhs = Vector6::new(
-            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        ) * velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
-
-            let lin_err = anchor2.translation.vector - anchor1.translation.vector;
-            let ang_err = anchor2.rotation * anchor1.rotation.inverse();
-
-            #[cfg(feature = "dim2")]
-            {
-                let ang_err = ang_err.angle();
-                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
-            }
-
-            #[cfg(feature = "dim3")]
-            {
-                let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
-                rhs += Vector6::new(
-                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
-                ) * velocity_based_erp_inv_dt;
-            }
-        }
-
-        WFixedVelocityConstraint {
-            joint_id,
-            mj_lambda1,
-            mj_lambda2,
-            im1,
-            im2,
-            ii1,
-            ii2,
-            ii1_sqrt,
-            ii2_sqrt,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            inv_lhs,
-            r1,
-            r2,
-            rhs,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<3>(3).into_owned();
-
-        mj_lambda1.linear += lin_impulse * self.im1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-        let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
-            + mj_lambda2.linear
-            + ang_vel2.gcross(self.r2);
-        let dangvel = -ang_vel1 + ang_vel2;
-
-        #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
-        ) + self.rhs;
-
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<3>(3).into_owned();
-
-        mj_lambda1.linear += lin_impulse * self.im1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::FixedJoint(fixed) = &mut joint.params {
-                fixed.impulse = self.impulse.extract(ii)
-            }
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WFixedVelocityGroundConstraint {
-    mj_lambda2: [usize; SIMD_WIDTH],
-
-    joint_id: [JointIndex; SIMD_WIDTH],
-
-    impulse: SpacialVector<SimdReal>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
-    #[cfg(feature = "dim3")]
-    rhs: Vector6<SimdReal>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<SimdReal>,
-    #[cfg(feature = "dim2")]
-    rhs: Vector3<SimdReal>,
-
-    im2: SimdReal,
-    ii2: AngularInertia<SimdReal>,
-    ii2_sqrt: AngularInertia<SimdReal>,
-    r2: Vector<SimdReal>,
-}
-
-impl WFixedVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&FixedJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        let (poss1, vels1, mprops1) = rbs1;
-        let (poss2, vels2, mprops2, ids2) = rbs2;
-
-        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
-        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
-
-        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
-        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
-
-        let local_frame1 = Isometry::from(gather![|ii| if flipped[ii] {
-            cparams[ii].local_frame2
-        } else {
-            cparams[ii].local_frame1
-        }]);
-        let local_frame2 = Isometry::from(gather![|ii| if flipped[ii] {
-            cparams[ii].local_frame1
-        } else {
-            cparams[ii].local_frame2
-        }]);
-        let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
-
-        let anchor1 = position1 * local_frame1;
-        let anchor2 = position2 * local_frame2;
-        let ii2 = ii2_sqrt.squared();
-        let r1 = anchor1.translation.vector - world_com1.coords;
-        let r2 = anchor2.translation.vector - world_com2.coords;
-        let rmat2: CrossMatrix<_> = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D.
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2);
-            let lhs10 = ii2.transform_matrix(&rmat2);
-            let lhs11 = ii2.into_matrix();
-
-            lhs = Matrix6::zeros();
-            lhs.fixed_slice_mut::<3, 3>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<3, 3>(3, 3).copy_from(&lhs11);
-        }
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        {
-            let m11 = im2 + rmat2.x * rmat2.x * ii2;
-            let m12 = rmat2.x * rmat2.y * ii2;
-            let m22 = im2 + rmat2.y * rmat2.y * ii2;
-            let m13 = rmat2.x * ii2;
-            let m23 = rmat2.y * ii2;
-            let m33 = ii2;
-            lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
-        }
-
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix?
-        #[cfg(feature = "dim3")]
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
-        let ang_dvel = angvel2 - angvel1;
-
-        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
-
-        #[cfg(feature = "dim2")]
-        let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction;
-
-        #[cfg(feature = "dim3")]
-        let mut rhs = Vector6::new(
-            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        ) * velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
-
-            let lin_err = anchor2.translation.vector - anchor1.translation.vector;
-            let ang_err = anchor2.rotation * anchor1.rotation.inverse();
-
-            #[cfg(feature = "dim2")]
-            {
-                let ang_err = ang_err.angle();
-                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
-            }
-
-            #[cfg(feature = "dim3")]
-            {
-                let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
-                rhs += Vector6::new(
-                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
-                ) * velocity_based_erp_inv_dt;
-            }
-        }
-
-        WFixedVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2,
-            im2,
-            ii2,
-            ii2_sqrt,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            inv_lhs,
-            r2,
-            rhs,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<3>(3).into_owned();
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let dangvel = ang_vel2;
-        #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
-        ) + self.rhs;
-
-        let impulse = self.inv_lhs * rhs;
-
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<3>(3).into_owned();
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        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);
-        }
-    }
-
-    // FIXME: duplicated code with the non-ground constraint.
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::FixedJoint(fixed) = &mut joint.params {
-                fixed.impulse = self.impulse.extract(ii)
-            }
-        }
-    }
-}

src/dynamics/solver/joint_constraint/generic_position_constraint.rs

@@ -1,346 +0,0 @@
-use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{GenericJoint, IntegrationParameters};
-use crate::math::{
-    AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
-    DIM,
-};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-use na::{Vector3, Vector6};
-
-// FIXME: review this code for the case where the center of masses are not the origin.
-#[derive(Debug)]
-pub(crate) struct GenericPositionConstraint {
-    position1: usize,
-    position2: usize,
-    local_anchor1: Isometry<Real>,
-    local_anchor2: Isometry<Real>,
-    local_com1: Point<Real>,
-    local_com2: Point<Real>,
-    im1: Real,
-    im2: Real,
-    ii1: AngularInertia<Real>,
-    ii2: AngularInertia<Real>,
-
-    joint: GenericJoint,
-}
-
-impl GenericPositionConstraint {
-    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, joint: &GenericJoint) -> Self {
-        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let im1 = rb1.effective_inv_mass;
-        let im2 = rb2.effective_inv_mass;
-
-        Self {
-            local_anchor1: joint.local_anchor1,
-            local_anchor2: joint.local_anchor2,
-            position1: rb1.active_set_offset,
-            position2: rb2.active_set_offset,
-            im1,
-            im2,
-            ii1,
-            ii2,
-            local_com1: rb1.local_mprops.local_com,
-            local_com2: rb2.local_mprops.local_com,
-            joint: *joint,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position1 = positions[self.position1];
-        let mut position2 = positions[self.position2];
-        let mut params = *params;
-        params.joint_erp = 0.8;
-
-        /*
-         *
-         * Translation part.
-         *
-         */
-        {
-            let anchor1 = position1 * self.joint.local_anchor1;
-            let anchor2 = position2 * self.joint.local_anchor2;
-            let basis = anchor1.rotation;
-            let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1;
-            let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
-            let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
-            let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
-
-            let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
-                &anchor1,
-                &anchor2,
-                &basis,
-                &self.joint.min_position.xyz(),
-                &self.joint.max_position.xyz(),
-            ) * params.joint_erp;
-
-            for i in 0..3 {
-                if pos_rhs[i] < 0.0 {
-                    min_pos_impulse[i] = -Real::MAX;
-                }
-                if pos_rhs[i] > 0.0 {
-                    max_pos_impulse[i] = Real::MAX;
-                }
-            }
-
-            let rotmat = basis.to_rotation_matrix().into_inner();
-            let rmat1 = r1.gcross_matrix() * rotmat;
-            let rmat2 = r2.gcross_matrix() * rotmat;
-
-            // Will be actually inverted right after.
-            // TODO: we should keep the SdpMatrix3 type.
-            let delassus = (self.ii1.quadform(&rmat1).add_diagonal(self.im1)
-                + self.ii2.quadform(&rmat2).add_diagonal(self.im2))
-            .into_matrix();
-
-            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
-                &min_pos_impulse,
-                &max_pos_impulse,
-                &mut Vector3::zeros(),
-                delassus,
-            );
-
-            let local_impulse = (inv_delassus * pos_rhs)
-                .inf(&max_pos_impulse)
-                .sup(&min_pos_impulse);
-            let impulse = basis * local_impulse;
-
-            let rot1 = self.ii1.transform_vector(r1.gcross(impulse));
-            let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
-
-            position1.translation.vector += self.im1 * impulse;
-            position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
-            position2.translation.vector -= self.im2 * impulse;
-            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
-        }
-
-        /*
-         *
-         * Rotation part
-         *
-         */
-        {
-            let anchor1 = position1 * self.joint.local_anchor1;
-            let anchor2 = position2 * self.joint.local_anchor2;
-            let basis = anchor1.rotation;
-            let mut min_pos_impulse = self
-                .joint
-                .min_pos_impulse
-                .fixed_rows::<Dim>(DIM)
-                .into_owned();
-            let mut max_pos_impulse = self
-                .joint
-                .max_pos_impulse
-                .fixed_rows::<Dim>(DIM)
-                .into_owned();
-
-            let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
-                &anchor1,
-                &anchor2,
-                &basis,
-                &self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
-                &self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
-            ) * params.joint_erp;
-
-            for i in 0..3 {
-                if pos_rhs[i] < 0.0 {
-                    min_pos_impulse[i] = -Real::MAX;
-                }
-                if pos_rhs[i] > 0.0 {
-                    max_pos_impulse[i] = Real::MAX;
-                }
-            }
-
-            // TODO: we should keep the SdpMatrix3 type.
-            let rotmat = basis.to_rotation_matrix().into_inner();
-            let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix();
-
-            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
-                &min_pos_impulse,
-                &max_pos_impulse,
-                &mut Vector3::zeros(),
-                delassus,
-            );
-
-            let local_impulse = (inv_delassus * pos_rhs)
-                .inf(&max_pos_impulse)
-                .sup(&min_pos_impulse);
-            let impulse = basis * local_impulse;
-
-            let rot1 = self.ii1.transform_vector(impulse);
-            let rot2 = self.ii2.transform_vector(impulse);
-
-            position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
-            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
-        }
-
-        positions[self.position1] = position1;
-        positions[self.position2] = position2;
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct GenericPositionGroundConstraint {
-    position2: usize,
-    anchor1: Isometry<Real>,
-    local_anchor2: Isometry<Real>,
-    local_com2: Point<Real>,
-    im2: Real,
-    ii2: AngularInertia<Real>,
-    joint: GenericJoint,
-}
-
-impl GenericPositionGroundConstraint {
-    pub fn from_params(
-        rb1: &RigidBody,
-        rb2: &RigidBody,
-        joint: &GenericJoint,
-        flipped: bool,
-    ) -> Self {
-        let anchor1;
-        let local_anchor2;
-
-        if flipped {
-            anchor1 = rb1.predicted_position * joint.local_anchor2;
-            local_anchor2 = joint.local_anchor1;
-        } else {
-            anchor1 = rb1.predicted_position * joint.local_anchor1;
-            local_anchor2 = joint.local_anchor2;
-        };
-
-        Self {
-            anchor1,
-            local_anchor2,
-            position2: rb2.active_set_offset,
-            im2: rb2.effective_inv_mass,
-            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
-            local_com2: rb2.local_mprops.local_com,
-            joint: *joint,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position2 = positions[self.position2];
-        let mut params = *params;
-        params.joint_erp = 0.8;
-
-        /*
-         *
-         * Translation part.
-         *
-         */
-        {
-            let anchor1 = self.anchor1;
-            let anchor2 = position2 * self.local_anchor2;
-            let basis = anchor1.rotation;
-            let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
-            let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
-            let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
-
-            let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
-                &anchor1,
-                &anchor2,
-                &basis,
-                &self.joint.min_position.xyz(),
-                &self.joint.max_position.xyz(),
-            ) * params.joint_erp;
-
-            for i in 0..3 {
-                if pos_rhs[i] < 0.0 {
-                    min_pos_impulse[i] = -Real::MAX;
-                }
-                if pos_rhs[i] > 0.0 {
-                    max_pos_impulse[i] = Real::MAX;
-                }
-            }
-
-            let rotmat = basis.to_rotation_matrix().into_inner();
-            let rmat2 = r2.gcross_matrix() * rotmat;
-
-            // TODO: we should keep the SdpMatrix3 type.
-            let delassus = self
-                .ii2
-                .quadform(&rmat2)
-                .add_diagonal(self.im2)
-                .into_matrix();
-
-            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
-                &min_pos_impulse,
-                &max_pos_impulse,
-                &mut Vector3::zeros(),
-                delassus,
-            );
-
-            let local_impulse = (inv_delassus * pos_rhs)
-                .inf(&max_pos_impulse)
-                .sup(&min_pos_impulse);
-            let impulse = basis * local_impulse;
-
-            let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
-
-            position2.translation.vector -= self.im2 * impulse;
-            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
-        }
-
-        /*
-         *
-         * Rotation part
-         *
-         */
-        {
-            let anchor1 = self.anchor1;
-            let anchor2 = position2 * self.local_anchor2;
-            let basis = anchor1.rotation;
-            let mut min_pos_impulse = self
-                .joint
-                .min_pos_impulse
-                .fixed_rows::<Dim>(DIM)
-                .into_owned();
-            let mut max_pos_impulse = self
-                .joint
-                .max_pos_impulse
-                .fixed_rows::<Dim>(DIM)
-                .into_owned();
-
-            let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
-                &anchor1,
-                &anchor2,
-                &basis,
-                &self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
-                &self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
-            ) * params.joint_erp;
-
-            for i in 0..3 {
-                if pos_rhs[i] < 0.0 {
-                    min_pos_impulse[i] = -Real::MAX;
-                }
-                if pos_rhs[i] > 0.0 {
-                    max_pos_impulse[i] = Real::MAX;
-                }
-            }
-
-            // Will be actually inverted right after.
-            // TODO: we should keep the SdpMatrix3 type.
-            let rotmat = basis.to_rotation_matrix().into_inner();
-            let delassus = self.ii2.quadform(&rotmat).into_matrix();
-
-            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
-                &min_pos_impulse,
-                &max_pos_impulse,
-                &mut Vector3::zeros(),
-                delassus,
-            );
-
-            let local_impulse = (inv_delassus * pos_rhs)
-                .inf(&max_pos_impulse)
-                .sup(&min_pos_impulse);
-            let impulse = basis * local_impulse;
-            let rot2 = self.ii2.transform_vector(impulse);
-
-            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
-        }
-
-        positions[self.position2] = position2;
-    }
-}

src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs

@@ -1,60 +0,0 @@
-use super::{GenericPositionConstraint, GenericPositionGroundConstraint};
-use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
-use crate::math::{Isometry, Real, SIMD_WIDTH};
-
-// TODO: this does not uses SIMD optimizations yet.
-#[derive(Debug)]
-pub(crate) struct WGenericPositionConstraint {
-    constraints: [GenericPositionConstraint; SIMD_WIDTH],
-}
-
-impl WGenericPositionConstraint {
-    pub fn from_params(
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&GenericJoint; SIMD_WIDTH],
-    ) -> Self {
-        Self {
-            constraints: gather![|ii| GenericPositionConstraint::from_params(
-                rbs1[ii],
-                rbs2[ii],
-                cparams[ii]
-            )],
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        for constraint in &self.constraints {
-            constraint.solve(params, positions);
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WGenericPositionGroundConstraint {
-    constraints: [GenericPositionGroundConstraint; SIMD_WIDTH],
-}
-
-impl WGenericPositionGroundConstraint {
-    pub fn from_params(
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&GenericJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        Self {
-            constraints: gather![|ii| GenericPositionGroundConstraint::from_params(
-                rbs1[ii],
-                rbs2[ii],
-                cparams[ii],
-                flipped[ii]
-            )],
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        for constraint in &self.constraints {
-            constraint.solve(params, positions);
-        }
-    }
-}

src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs

@@ -1,706 +0,0 @@
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
-};
-use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, Vector, DIM};
-use crate::na::UnitQuaternion;
-use crate::parry::math::{AngDim, SpatialVector};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-#[cfg(feature = "dim3")]
-use na::{Matrix3, Matrix6, Vector3, Vector6, U3};
-#[cfg(feature = "dim2")]
-use na::{Matrix3, Vector3};
-
-#[derive(Debug)]
-pub(crate) struct GenericVelocityConstraint {
-    mj_lambda1: usize,
-    mj_lambda2: usize,
-
-    joint_id: JointIndex,
-
-    inv_lhs_lin: Matrix3<Real>,
-    inv_lhs_ang: Matrix3<Real>,
-
-    im1: Real,
-    im2: Real,
-
-    ii1: AngularInertia<Real>,
-    ii2: AngularInertia<Real>,
-
-    ii1_sqrt: AngularInertia<Real>,
-    ii2_sqrt: AngularInertia<Real>,
-
-    r1: Vector<Real>,
-    r2: Vector<Real>,
-    basis1: Rotation<Real>,
-    basis2: Rotation<Real>,
-    dependant_set_mask: SpacialVector<Real>,
-
-    vel: GenericConstraintPart,
-}
-
-impl GenericVelocityConstraint {
-    pub fn compute_velocity_error(
-        min_velocity: &SpatialVector<Real>,
-        max_velocity: &SpatialVector<Real>,
-        r1: &Vector<Real>,
-        r2: &Vector<Real>,
-        basis1: &Rotation<Real>,
-        basis2: &Rotation<Real>,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
-    ) -> SpatialVector<Real> {
-        let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel() - rb1.angvel().gcross(*r1)))
-            + basis2.inverse_transform_vector(&(rb2.linvel() + rb2.angvel().gcross(*r2)));
-        let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
-            + basis2.inverse_transform_vector(&rb2.angvel);
-
-        let min_linvel = min_velocity.xyz();
-        let min_angvel = min_velocity.fixed_rows::<AngDim>(DIM).into_owned();
-        let max_linvel = max_velocity.xyz();
-        let max_angvel = max_velocity.fixed_rows::<AngDim>(DIM).into_owned();
-        let lin_rhs = lin_dvel - lin_dvel.sup(&min_linvel).inf(&max_linvel);
-        let ang_rhs = ang_dvel - ang_dvel.sup(&min_angvel).inf(&max_angvel);
-
-        #[cfg(feature = "dim2")]
-        return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs);
-
-        #[cfg(feature = "dim3")]
-        return Vector6::new(
-            lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y, ang_rhs.z,
-        );
-    }
-
-    pub fn compute_lin_position_error(
-        anchor1: &Isometry<Real>,
-        anchor2: &Isometry<Real>,
-        basis: &Rotation<Real>,
-        min_position: &Vector<Real>,
-        max_position: &Vector<Real>,
-    ) -> Vector<Real> {
-        let dpos = anchor2.translation.vector - anchor1.translation.vector;
-        let local_dpos = basis.inverse_transform_vector(&dpos);
-        local_dpos - local_dpos.sup(min_position).inf(max_position)
-    }
-
-    pub fn compute_ang_position_error(
-        anchor1: &Isometry<Real>,
-        anchor2: &Isometry<Real>,
-        basis: &Rotation<Real>,
-        min_position: &Vector<Real>,
-        max_position: &Vector<Real>,
-    ) -> Vector<Real> {
-        let drot = anchor2.rotation * anchor1.rotation.inverse();
-        let local_drot_diff = basis.inverse_transform_vector(&drot.scaled_axis());
-
-        let error = local_drot_diff - local_drot_diff.sup(min_position).inf(max_position);
-        let error_code =
-            (error[0] == 0.0) as usize + (error[1] == 0.0) as usize + (error[2] == 0.0) as usize;
-
-        if error_code == 1 {
-            // Align two axes.
-            let constrained_axis = error.iamin();
-            let axis1 = anchor1
-                .rotation
-                .to_rotation_matrix()
-                .into_inner()
-                .column(constrained_axis)
-                .into_owned();
-            let axis2 = anchor2
-                .rotation
-                .to_rotation_matrix()
-                .into_inner()
-                .column(constrained_axis)
-                .into_owned();
-            let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2)
-                .unwrap_or(UnitQuaternion::identity());
-            let local_drot_diff = basis.inverse_transform_vector(&rot_cross.scaled_axis());
-            local_drot_diff - local_drot_diff.sup(min_position).inf(max_position)
-        } else {
-            error
-        }
-    }
-
-    pub fn invert_partial_delassus_matrix(
-        min_impulse: &Vector<Real>,
-        max_impulse: &Vector<Real>,
-        dependant_set_mask: &mut Vector<Real>,
-        mut delassus: na::Matrix3<Real>,
-    ) -> na::Matrix3<Real> {
-        // Adjust the Delassus matrix to take force limits into account.
-        // If a DoF has a force limit, then we need to make its
-        // constraint independent from the others because otherwise
-        // the force clamping will cause errors to propagate in the
-        // other constraints.
-        for i in 0..3 {
-            if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX {
-                let diag = delassus[(i, i)];
-                delassus.column_mut(i).fill(0.0);
-                delassus.row_mut(i).fill(0.0);
-                delassus[(i, i)] = diag;
-                dependant_set_mask[i] = 0.0;
-            } else {
-                dependant_set_mask[i] = 1.0;
-            }
-        }
-
-        delassus.try_inverse().unwrap()
-    }
-
-    pub fn compute_position_error(
-        joint: &GenericJoint,
-        anchor1: &Isometry<Real>,
-        anchor2: &Isometry<Real>,
-        basis: &Rotation<Real>,
-    ) -> SpatialVector<Real> {
-        let delta_pos = Isometry::from_parts(
-            anchor2.translation * anchor1.translation.inverse(),
-            anchor2.rotation * anchor1.rotation.inverse(),
-        );
-        let lin_dpos = basis.inverse_transform_vector(&delta_pos.translation.vector);
-        let ang_dpos = basis.inverse_transform_vector(&delta_pos.rotation.scaled_axis());
-
-        let dpos = Vector6::new(
-            lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
-        );
-
-        let error = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position);
-        let error_code =
-            (error[3] == 0.0) as usize + (error[4] == 0.0) as usize + (error[5] == 0.0) as usize;
-
-        match error_code {
-            1 => {
-                let constrained_axis = error.rows(3, 3).iamin();
-                let axis1 = anchor1
-                    .rotation
-                    .to_rotation_matrix()
-                    .into_inner()
-                    .column(constrained_axis)
-                    .into_owned();
-                let axis2 = anchor2
-                    .rotation
-                    .to_rotation_matrix()
-                    .into_inner()
-                    .column(constrained_axis)
-                    .into_owned();
-                let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2)
-                    .unwrap_or(UnitQuaternion::identity());
-                let ang_dpos = basis.inverse_transform_vector(&rot_cross.scaled_axis());
-                let dpos = Vector6::new(
-                    lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
-                );
-
-                dpos - dpos.sup(&joint.min_position).inf(&joint.max_position)
-            }
-            _ => error,
-        }
-    }
-
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
-        joint: &GenericJoint,
-    ) -> Self {
-        let anchor1 = rb1.position() * joint.local_anchor1;
-        let anchor2 = rb2.position() * joint.local_anchor2;
-        let basis1 = anchor1.rotation;
-        let basis2 = anchor2.rotation;
-        let im1 = rb1.effective_inv_mass;
-        let im2 = rb2.effective_inv_mass;
-        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1.translation.vector - rb1.world_com.coords;
-        let r2 = anchor2.translation.vector - rb2.world_com.coords;
-        let mut min_impulse = joint.min_impulse;
-        let mut max_impulse = joint.max_impulse;
-        let mut min_pos_impulse = joint.min_pos_impulse;
-        let mut max_pos_impulse = joint.max_pos_impulse;
-        let mut min_velocity = joint.min_velocity;
-        let mut max_velocity = joint.max_velocity;
-        let mut dependant_set_mask = SpacialVector::repeat(1.0);
-
-        let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis1)
-            * params.inv_dt()
-            * params.joint_erp;
-
-        for i in 0..6 {
-            if pos_rhs[i] < 0.0 {
-                min_impulse[i] = -Real::MAX;
-                min_pos_impulse[i] = -Real::MAX;
-                min_velocity[i] = 0.0;
-            }
-            if pos_rhs[i] > 0.0 {
-                max_impulse[i] = Real::MAX;
-                max_pos_impulse[i] = Real::MAX;
-                max_velocity[i] = 0.0;
-            }
-        }
-
-        let rhs = Self::compute_velocity_error(
-            &min_velocity,
-            &max_velocity,
-            &r1,
-            &r2,
-            &basis1,
-            &basis2,
-            rb1,
-            rb2,
-        );
-        let rhs_lin = rhs.xyz();
-        let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into();
-
-        // TODO: we should keep the SdpMatrix3 type.
-        let rotmat1 = basis1.to_rotation_matrix().into_inner();
-        let rotmat2 = basis2.to_rotation_matrix().into_inner();
-        let rmat1 = r1.gcross_matrix() * rotmat1;
-        let rmat2 = r2.gcross_matrix() * rotmat2;
-        let delassus00 = (ii1.quadform(&rmat1).add_diagonal(im1)
-            + ii2.quadform(&rmat2).add_diagonal(im2))
-        .into_matrix();
-        let delassus11 = (ii1.quadform(&rotmat1) + ii2.quadform(&rotmat2)).into_matrix();
-
-        let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
-            &min_pos_impulse.xyz(),
-            &max_pos_impulse.xyz(),
-            &mut Vector3::zeros(),
-            delassus00,
-        );
-        let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix(
-            &min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
-            &max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
-            &mut Vector3::zeros(),
-            delassus11,
-        );
-
-        let impulse = (joint.impulse * params.warmstart_coeff)
-            .inf(&max_impulse)
-            .sup(&min_impulse);
-
-        let lin_impulse = impulse.xyz();
-        let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned();
-        let min_lin_impulse = min_impulse.xyz();
-        let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned();
-        let max_lin_impulse = max_impulse.xyz();
-        let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned();
-
-        GenericVelocityConstraint {
-            joint_id,
-            mj_lambda1: rb1.active_set_offset,
-            mj_lambda2: rb2.active_set_offset,
-            im1,
-            im2,
-            ii1,
-            ii2,
-            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
-            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            inv_lhs_lin,
-            inv_lhs_ang,
-            r1,
-            r2,
-            basis1,
-            basis2,
-            dependant_set_mask,
-            vel: GenericConstraintPart {
-                lin_impulse,
-                ang_impulse,
-                min_lin_impulse,
-                min_ang_impulse,
-                max_lin_impulse,
-                max_ang_impulse,
-                rhs_lin,
-                rhs_ang,
-            },
-        }
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let lin_impulse1 = self.basis1 * self.vel.lin_impulse;
-        let ang_impulse1 = self.basis1 * self.vel.ang_impulse;
-        let lin_impulse2 = self.basis2 * self.vel.lin_impulse;
-        let ang_impulse2 = self.basis2 * self.vel.ang_impulse;
-
-        mj_lambda1.linear += self.im1 * lin_impulse1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
-
-        mj_lambda2.linear -= self.im2 * lin_impulse2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let (lin_imp, ang_imp) = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2);
-        self.vel.lin_impulse = lin_imp;
-        self.vel.ang_impulse = ang_imp;
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        match &mut joint.params {
-            JointParams::GenericJoint(out) => {
-                out.impulse[0] = self.vel.lin_impulse.x;
-                out.impulse[1] = self.vel.lin_impulse.y;
-                out.impulse[2] = self.vel.lin_impulse.z;
-                out.impulse[3] = self.vel.ang_impulse.x;
-                out.impulse[4] = self.vel.ang_impulse.y;
-                out.impulse[5] = self.vel.ang_impulse.z;
-            }
-            JointParams::RevoluteJoint(out) => {
-                out.impulse[0] = self.vel.lin_impulse.x;
-                out.impulse[1] = self.vel.lin_impulse.y;
-                out.impulse[2] = self.vel.lin_impulse.z;
-                out.motor_impulse = self.vel.ang_impulse.x;
-                out.impulse[3] = self.vel.ang_impulse.y;
-                out.impulse[4] = self.vel.ang_impulse.z;
-            }
-            _ => unimplemented!(),
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct GenericVelocityGroundConstraint {
-    mj_lambda2: usize,
-
-    joint_id: JointIndex,
-
-    inv_lhs_lin: Matrix3<Real>,
-    inv_lhs_ang: Matrix3<Real>,
-
-    im2: Real,
-    ii2: AngularInertia<Real>,
-    ii2_sqrt: AngularInertia<Real>,
-    r2: Vector<Real>,
-    basis: Rotation<Real>,
-
-    dependant_set_mask: SpacialVector<Real>,
-
-    vel: GenericConstraintPart,
-}
-
-impl GenericVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: &RigidBody,
-        rb2: &RigidBody,
-        joint: &GenericJoint,
-        flipped: bool,
-    ) -> Self {
-        let (anchor1, anchor2) = if flipped {
-            (
-                rb1.position() * joint.local_anchor2,
-                rb2.position() * joint.local_anchor1,
-            )
-        } else {
-            (
-                rb1.position() * joint.local_anchor1,
-                rb2.position() * joint.local_anchor2,
-            )
-        };
-
-        let basis = anchor2.rotation;
-        let im2 = rb2.effective_inv_mass;
-        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1.translation.vector - rb1.world_com.coords;
-        let r2 = anchor2.translation.vector - rb2.world_com.coords;
-        let mut min_impulse = joint.min_impulse;
-        let mut max_impulse = joint.max_impulse;
-        let mut min_pos_impulse = joint.min_pos_impulse;
-        let mut max_pos_impulse = joint.max_pos_impulse;
-        let mut min_velocity = joint.min_velocity;
-        let mut max_velocity = joint.max_velocity;
-        let mut dependant_set_mask = SpacialVector::repeat(1.0);
-
-        let pos_rhs =
-            GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2, &basis)
-                * params.inv_dt()
-                * params.joint_erp;
-
-        for i in 0..6 {
-            if pos_rhs[i] < 0.0 {
-                min_impulse[i] = -Real::MAX;
-                min_pos_impulse[i] = -Real::MAX;
-                min_velocity[i] = 0.0;
-            }
-            if pos_rhs[i] > 0.0 {
-                max_impulse[i] = Real::MAX;
-                max_pos_impulse[i] = Real::MAX;
-                max_velocity[i] = 0.0;
-            }
-        }
-
-        let rhs = GenericVelocityConstraint::compute_velocity_error(
-            &min_velocity,
-            &max_velocity,
-            &r1,
-            &r2,
-            &basis,
-            &basis,
-            rb1,
-            rb2,
-        );
-        let rhs_lin = rhs.xyz();
-        let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into_owned();
-
-        // TODO: we should keep the SdpMatrix3 type.
-        let rotmat = basis.to_rotation_matrix().into_inner();
-        let rmat2 = r2.gcross_matrix() * rotmat;
-        let delassus00 = ii2.quadform(&rmat2).add_diagonal(im2).into_matrix();
-        let delassus11 = ii2.quadform(&rotmat).into_matrix();
-
-        let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
-            &min_pos_impulse.xyz(),
-            &max_pos_impulse.xyz(),
-            &mut Vector3::zeros(),
-            delassus00,
-        );
-        let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix(
-            &min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
-            &max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
-            &mut Vector3::zeros(),
-            delassus11,
-        );
-
-        let impulse = (joint.impulse * params.warmstart_coeff)
-            .inf(&max_impulse)
-            .sup(&min_impulse);
-
-        let lin_impulse = impulse.xyz();
-        let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned();
-        let min_lin_impulse = min_impulse.xyz();
-        let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned();
-        let max_lin_impulse = max_impulse.xyz();
-        let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned();
-
-        GenericVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2: rb2.active_set_offset,
-            im2,
-            ii2,
-            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            inv_lhs_lin,
-            inv_lhs_ang,
-            r2,
-            basis,
-            vel: GenericConstraintPart {
-                lin_impulse,
-                ang_impulse,
-                min_lin_impulse,
-                min_ang_impulse,
-                max_lin_impulse,
-                max_ang_impulse,
-                rhs_lin,
-                rhs_ang,
-            },
-            dependant_set_mask,
-        }
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let lin_impulse = self.basis * self.vel.lin_impulse;
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.basis * self.vel.impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.basis * self.vel.ang_impulse;
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let (lin_imp, ang_imp) = self.vel.solve_ground(self, &mut mj_lambda2);
-        self.vel.lin_impulse = lin_imp;
-        self.vel.ang_impulse = ang_imp;
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    // TODO: duplicated code with the non-ground constraint.
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        match &mut joint.params {
-            JointParams::GenericJoint(out) => {
-                out.impulse[0] = self.vel.lin_impulse.x;
-                out.impulse[1] = self.vel.lin_impulse.y;
-                out.impulse[2] = self.vel.lin_impulse.z;
-                out.impulse[3] = self.vel.ang_impulse.x;
-                out.impulse[4] = self.vel.ang_impulse.y;
-                out.impulse[5] = self.vel.ang_impulse.z;
-            }
-            JointParams::RevoluteJoint(out) => {
-                out.impulse[0] = self.vel.lin_impulse.x;
-                out.impulse[1] = self.vel.lin_impulse.y;
-                out.impulse[2] = self.vel.lin_impulse.z;
-                out.motor_impulse = self.vel.ang_impulse.x;
-                out.impulse[3] = self.vel.ang_impulse.y;
-                out.impulse[4] = self.vel.ang_impulse.z;
-            }
-            _ => unimplemented!(),
-        }
-    }
-}
-
-#[derive(Debug)]
-struct GenericConstraintPart {
-    lin_impulse: Vector3<Real>,
-    max_lin_impulse: Vector3<Real>,
-    min_lin_impulse: Vector3<Real>,
-    rhs_lin: Vector3<Real>,
-
-    ang_impulse: Vector3<Real>,
-    max_ang_impulse: Vector3<Real>,
-    min_ang_impulse: Vector3<Real>,
-    rhs_ang: Vector3<Real>,
-}
-
-impl GenericConstraintPart {
-    fn solve(
-        &self,
-        parent: &GenericVelocityConstraint,
-        mj_lambda1: &mut DeltaVel<Real>,
-        mj_lambda2: &mut DeltaVel<Real>,
-    ) -> (Vector3<Real>, Vector3<Real>) {
-        let new_lin_impulse;
-        let new_ang_impulse;
-
-        /*
-         *
-         * Solve translations.
-         *
-         */
-        {
-            let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
-            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let dvel = parent
-                .basis1
-                .inverse_transform_vector(&(-mj_lambda1.linear - ang_vel1.gcross(parent.r1)))
-                + parent
-                    .basis2
-                    .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
-
-            let err = dvel + self.rhs_lin;
-
-            new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
-                .sup(&self.min_lin_impulse)
-                .inf(&self.max_lin_impulse);
-            let effective_impulse1 = parent.basis1 * (new_lin_impulse - self.lin_impulse);
-            let effective_impulse2 = parent.basis2 * (new_lin_impulse - self.lin_impulse);
-
-            mj_lambda1.linear += parent.im1 * effective_impulse1;
-            mj_lambda1.angular += parent
-                .ii1_sqrt
-                .transform_vector(parent.r1.gcross(effective_impulse1));
-
-            mj_lambda2.linear -= parent.im2 * effective_impulse2;
-            mj_lambda2.angular -= parent
-                .ii2_sqrt
-                .transform_vector(parent.r2.gcross(effective_impulse2));
-        }
-
-        /*
-         *
-         * Solve rotations.
-         *
-         */
-        {
-            let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
-            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let dvel = parent.basis2.inverse_transform_vector(&ang_vel2)
-                - parent.basis1.inverse_transform_vector(&ang_vel1);
-            let err = dvel + self.rhs_ang;
-
-            new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
-                .sup(&self.min_ang_impulse)
-                .inf(&self.max_ang_impulse);
-            let effective_impulse1 = parent.basis1 * (new_ang_impulse - self.ang_impulse);
-            let effective_impulse2 = parent.basis2 * (new_ang_impulse - self.ang_impulse);
-
-            mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse1);
-            mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse2);
-        }
-
-        (new_lin_impulse, new_ang_impulse)
-    }
-
-    fn solve_ground(
-        &self,
-        parent: &GenericVelocityGroundConstraint,
-        mj_lambda2: &mut DeltaVel<Real>,
-    ) -> (Vector3<Real>, Vector3<Real>) {
-        let new_lin_impulse;
-        let new_ang_impulse;
-
-        /*
-         *
-         * Solve translations.
-         *
-         */
-        {
-            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let dvel = parent
-                .basis
-                .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
-
-            let err = dvel + self.rhs_lin;
-
-            new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
-                .sup(&self.min_lin_impulse)
-                .inf(&self.max_lin_impulse);
-            let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse);
-
-            mj_lambda2.linear -= parent.im2 * effective_impulse;
-            mj_lambda2.angular -= parent
-                .ii2_sqrt
-                .transform_vector(parent.r2.gcross(effective_impulse));
-        }
-
-        /*
-         *
-         * Solve rotations.
-         *
-         */
-        {
-            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let dvel = parent.basis.inverse_transform_vector(&ang_vel2);
-            let err = dvel + self.rhs_ang;
-
-            new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
-                .sup(&self.min_ang_impulse)
-                .inf(&self.max_ang_impulse);
-            let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse);
-
-            mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse);
-        }
-
-        (new_lin_impulse, new_ang_impulse)
-    }
-}

src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs

@@ -1,464 +0,0 @@
-use simba::simd::SimdValue;
-
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
-};
-use crate::math::{
-    AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector,
-    Vector, SIMD_WIDTH,
-};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-#[cfg(feature = "dim3")]
-use na::{Cholesky, Matrix6, Vector6, U3};
-#[cfg(feature = "dim2")]
-use {
-    na::{Matrix3, Vector3},
-    parry::utils::SdpMatrix3,
-};
-
-#[derive(Debug)]
-pub(crate) struct WGenericVelocityConstraint {
-    mj_lambda1: [usize; SIMD_WIDTH],
-    mj_lambda2: [usize; SIMD_WIDTH],
-
-    joint_id: [JointIndex; SIMD_WIDTH],
-
-    impulse: SpacialVector<SimdReal>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
-    #[cfg(feature = "dim3")]
-    rhs: Vector6<SimdReal>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<SimdReal>,
-    #[cfg(feature = "dim2")]
-    rhs: Vector3<SimdReal>,
-
-    im1: SimdReal,
-    im2: SimdReal,
-
-    ii1: AngularInertia<SimdReal>,
-    ii2: AngularInertia<SimdReal>,
-
-    ii1_sqrt: AngularInertia<SimdReal>,
-    ii2_sqrt: AngularInertia<SimdReal>,
-
-    r1: Vector<SimdReal>,
-    r2: Vector<SimdReal>,
-}
-
-impl WGenericVelocityConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&GenericJoint; SIMD_WIDTH],
-    ) -> Self {
-        let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
-        let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
-        let im1 = SimdReal::from(gather![|ii| rbs1[ii].effective_inv_mass]);
-        let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| rbs1[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda1 = gather![|ii| rbs1[ii].active_set_offset];
-
-        let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
-        let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| rbs2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
-
-        let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
-        let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
-        let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
-
-        let anchor1 = position1 * local_anchor1;
-        let anchor2 = position2 * local_anchor2;
-        let ii1 = ii1_sqrt.squared();
-        let ii2 = ii2_sqrt.squared();
-        let r1 = anchor1.translation.vector - world_com1.coords;
-        let r2 = anchor2.translation.vector - world_com2.coords;
-        let rmat1: CrossMatrix<_> = r1.gcross_matrix();
-        let rmat2: CrossMatrix<_> = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D.
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let lhs00 =
-                ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2);
-            let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2);
-            let lhs11 = (ii1 + ii2).into_matrix();
-
-            // Note that Cholesky only reads the lower-triangular part of the matrix
-            // so we don't need to fill lhs01.
-            lhs = Matrix6::zeros();
-            lhs.fixed_slice_mut::<U3, U3>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
-        }
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        {
-            let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2;
-            let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2;
-            let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2;
-            let m13 = rmat1.x * ii1 + rmat2.x * ii2;
-            let m23 = rmat1.y * ii1 + rmat2.y * ii2;
-            let m33 = ii1 + ii2;
-            lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
-        }
-
-        // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix
-        // for which a textbook inverse is still efficient.
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't extract the matrix?
-        #[cfg(feature = "dim3")]
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2);
-        let ang_dvel = -angvel1 + angvel2;
-
-        #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
-
-        #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
-
-        WGenericVelocityConstraint {
-            joint_id,
-            mj_lambda1,
-            mj_lambda2,
-            im1,
-            im2,
-            ii1,
-            ii2,
-            ii1_sqrt,
-            ii2_sqrt,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            inv_lhs,
-            r1,
-            r2,
-            rhs,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
-
-        mj_lambda1.linear += lin_impulse * self.im1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-        let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
-            + mj_lambda2.linear
-            + ang_vel2.gcross(self.r2);
-        let dangvel = -ang_vel1 + ang_vel2;
-
-        #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
-        ) + self.rhs;
-
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
-
-        mj_lambda1.linear += lin_impulse * self.im1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::GenericJoint(fixed) = &mut joint.params {
-                fixed.impulse = self.impulse.extract(ii)
-            }
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WGenericVelocityGroundConstraint {
-    mj_lambda2: [usize; SIMD_WIDTH],
-
-    joint_id: [JointIndex; SIMD_WIDTH],
-
-    impulse: SpacialVector<SimdReal>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
-    #[cfg(feature = "dim3")]
-    rhs: Vector6<SimdReal>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<SimdReal>,
-    #[cfg(feature = "dim2")]
-    rhs: Vector3<SimdReal>,
-
-    im2: SimdReal,
-    ii2: AngularInertia<SimdReal>,
-    ii2_sqrt: AngularInertia<SimdReal>,
-    r2: Vector<SimdReal>,
-}
-
-impl WGenericVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: [&RigidBody; SIMD_WIDTH],
-        rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&GenericJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
-        let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
-
-        let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
-        let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| rbs2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
-
-        let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
-            cparams[ii].local_anchor2
-        } else {
-            cparams[ii].local_anchor1
-        }]);
-        let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
-            cparams[ii].local_anchor1
-        } else {
-            cparams[ii].local_anchor2
-        }]);
-        let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
-
-        let anchor1 = position1 * local_anchor1;
-        let anchor2 = position2 * local_anchor2;
-        let ii2 = ii2_sqrt.squared();
-        let r1 = anchor1.translation.vector - world_com1.coords;
-        let r2 = anchor2.translation.vector - world_com2.coords;
-        let rmat2: CrossMatrix<_> = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D.
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2);
-            let lhs10 = ii2.transform_matrix(&rmat2);
-            let lhs11 = ii2.into_matrix();
-
-            lhs = Matrix6::zeros();
-            lhs.fixed_slice_mut::<U3, U3>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
-        }
-
-        // In 2D we just unroll the computation because
-        // it's just easier that way.
-        #[cfg(feature = "dim2")]
-        {
-            let m11 = im2 + rmat2.x * rmat2.x * ii2;
-            let m12 = rmat2.x * rmat2.y * ii2;
-            let m22 = im2 + rmat2.y * rmat2.y * ii2;
-            let m13 = rmat2.x * ii2;
-            let m23 = rmat2.y * ii2;
-            let m33 = ii2;
-            lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
-        }
-
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix?
-        #[cfg(feature = "dim3")]
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
-        let ang_dvel = angvel2 - angvel1;
-
-        #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
-        #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
-
-        WGenericVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2,
-            im2,
-            ii2,
-            ii2_sqrt,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            inv_lhs,
-            r2,
-            rhs,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let dangvel = ang_vel2;
-        #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
-            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
-        ) + self.rhs;
-
-        let impulse = self.inv_lhs * rhs;
-
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        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);
-        }
-    }
-
-    // FIXME: duplicated code with the non-ground constraint.
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::GenericJoint(fixed) = &mut joint.params {
-                fixed.impulse = self.impulse.extract(ii)
-            }
-        }
-    }
-}

src/dynamics/solver/joint_constraint/joint_constraint.rs

@@ -1,118 +1,155 @@
-use super::{
-    BallVelocityConstraint, BallVelocityGroundConstraint, FixedVelocityConstraint,
-    FixedVelocityGroundConstraint, PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
-};
-#[cfg(feature = "dim3")]
-use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint};
-#[cfg(feature = "simd-is-enabled")]
-use super::{
-    WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint,
-    WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint,
-    WPrismaticVelocityGroundConstraint,
-};
-#[cfg(feature = "dim3")]
-#[cfg(feature = "simd-is-enabled")]
-use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
-// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
-//     GenericVelocityConstraint, GenericVelocityGroundConstraint,
-// };
 use crate::data::{BundleSet, ComponentSet};
+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::{
-    IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
+    ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
     RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
 };
-use crate::math::Real;
+use crate::math::{Isometry, Real, SPATIAL_DIM};
 #[cfg(feature = "simd-is-enabled")]
-use crate::math::SIMD_WIDTH;
+use crate::math::{SimdReal, SIMD_WIDTH};
+use crate::prelude::MultibodyJointSet;
+use na::DVector;
 
-pub(crate) enum AnyJointVelocityConstraint {
-    BallConstraint(BallVelocityConstraint),
-    BallGroundConstraint(BallVelocityGroundConstraint),
+pub enum AnyJointVelocityConstraint {
+    JointConstraint(JointVelocityConstraint<Real, 1>),
+    JointGroundConstraint(JointVelocityGroundConstraint<Real, 1>),
+    JointGenericConstraint(JointGenericVelocityConstraint),
+    JointGenericGroundConstraint(JointGenericVelocityGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
-    WBallConstraint(WBallVelocityConstraint),
+    JointConstraintSimd(JointVelocityConstraint<SimdReal, SIMD_WIDTH>),
     #[cfg(feature = "simd-is-enabled")]
-    WBallGroundConstraint(WBallVelocityGroundConstraint),
-    FixedConstraint(FixedVelocityConstraint),
-    FixedGroundConstraint(FixedVelocityGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WFixedConstraint(WFixedVelocityConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WFixedGroundConstraint(WFixedVelocityGroundConstraint),
-    // GenericConstraint(GenericVelocityConstraint),
-    // GenericGroundConstraint(GenericVelocityGroundConstraint),
-    // #[cfg(feature = "simd-is-enabled")]
-    // WGenericConstraint(WGenericVelocityConstraint),
-    // #[cfg(feature = "simd-is-enabled")]
-    // WGenericGroundConstraint(WGenericVelocityGroundConstraint),
-    PrismaticConstraint(PrismaticVelocityConstraint),
-    PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WPrismaticConstraint(WPrismaticVelocityConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WPrismaticGroundConstraint(WPrismaticVelocityGroundConstraint),
-    #[cfg(feature = "dim3")]
-    RevoluteConstraint(RevoluteVelocityConstraint),
-    #[cfg(feature = "dim3")]
-    RevoluteGroundConstraint(RevoluteVelocityGroundConstraint),
-    #[cfg(feature = "dim3")]
-    #[cfg(feature = "simd-is-enabled")]
-    WRevoluteConstraint(WRevoluteVelocityConstraint),
-    #[cfg(feature = "dim3")]
-    #[cfg(feature = "simd-is-enabled")]
-    WRevoluteGroundConstraint(WRevoluteVelocityGroundConstraint),
-    #[allow(dead_code)] // The Empty variant is only used with parallel code.
+    JointGroundConstraintSimd(JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH>),
     Empty,
 }
 
 impl AnyJointVelocityConstraint {
     #[cfg(feature = "parallel")]
-    pub fn num_active_constraints(_: &Joint) -> usize {
+    pub fn num_active_constraints(_: &ImpulseJoint) -> usize {
         1
     }
 
     pub fn from_joint<Bodies>(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        joint: &Joint,
+        joint: &ImpulseJoint,
         bodies: &Bodies,
-    ) -> Self
-    where
+        multibodies: &MultibodyJointSet,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+        out: &mut Vec<Self>,
+    ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
             + ComponentSet<RigidBodyMassProps>
             + ComponentSet<RigidBodyIds>,
     {
-        let rb1 = (
-            bodies.index(joint.body1.0),
-            bodies.index(joint.body1.0),
-            bodies.index(joint.body1.0),
-            bodies.index(joint.body1.0),
-        );
-        let rb2 = (
-            bodies.index(joint.body2.0),
-            bodies.index(joint.body2.0),
-            bodies.index(joint.body2.0),
-            bodies.index(joint.body2.0),
-        );
+        let local_frame1 = joint.data.local_frame1;
+        let local_frame2 = joint.data.local_frame2;
+        let rb1: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ) = bodies.index_bundle(joint.body1.0);
+        let rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ) = bodies.index_bundle(joint.body2.0);
 
-        match &joint.params {
-            JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint(
-                BallVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
-            ),
-            JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedConstraint(
-                FixedVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
-            ),
-            JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
-                PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
-            ),
-            // JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
-            //     GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
-            // ),
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
-                RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
-            ),
+        let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rb1;
+        let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2;
+        let frame1 = rb_pos1.position * local_frame1;
+        let frame2 = rb_pos2.position * local_frame2;
+
+        let body1 = SolverBody {
+            linvel: rb_vel1.linvel,
+            angvel: rb_vel1.angvel,
+            im: rb_mprops1.effective_inv_mass,
+            sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
+            world_com: rb_mprops1.world_com,
+            mj_lambda: [rb_ids1.active_set_offset],
+        };
+        let body2 = SolverBody {
+            linvel: rb_vel2.linvel,
+            angvel: rb_vel2.angvel,
+            im: rb_mprops2.effective_inv_mass,
+            sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
+            world_com: rb_mprops2.world_com,
+            mj_lambda: [rb_ids2.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 {
+                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,
+            );
+
+            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,
+            );
+
+            for c in out_tmp.into_iter().take(out_tmp_len) {
+                out.push(AnyJointVelocityConstraint::JointConstraint(c));
+            }
         }
     }
 
@@ -120,70 +157,96 @@ impl AnyJointVelocityConstraint {
     pub fn from_wide_joint<Bodies>(
         params: &IntegrationParameters,
         joint_id: [JointIndex; SIMD_WIDTH],
-        joints: [&Joint; SIMD_WIDTH],
+        impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
         bodies: &Bodies,
-    ) -> Self
-    where
+        out: &mut Vec<Self>,
+    ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
             + ComponentSet<RigidBodyMassProps>
             + ComponentSet<RigidBodyIds>,
     {
-        let rbs1 = (
-            gather![|ii| bodies.index(joints[ii].body1.0)],
-            gather![|ii| bodies.index(joints[ii].body1.0)],
-            gather![|ii| bodies.index(joints[ii].body1.0)],
-            gather![|ii| bodies.index(joints[ii].body1.0)],
+        let rbs1: (
+            [&RigidBodyPosition; SIMD_WIDTH],
+            [&RigidBodyVelocity; SIMD_WIDTH],
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ) = (
+            gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
+            gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
+            gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
+            gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
         );
-        let rbs2 = (
-            gather![|ii| bodies.index(joints[ii].body2.0)],
-            gather![|ii| bodies.index(joints[ii].body2.0)],
-            gather![|ii| bodies.index(joints[ii].body2.0)],
-            gather![|ii| bodies.index(joints[ii].body2.0)],
+        let rbs2: (
+            [&RigidBodyPosition; SIMD_WIDTH],
+            [&RigidBodyVelocity; SIMD_WIDTH],
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ) = (
+            gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
+            gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
+            gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
+            gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
         );
 
-        match &joints[0].params {
-            JointParams::BallJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
-                AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params(
-                    params, joint_id, rbs1, rbs2, joints,
-                ))
-            }
-            JointParams::FixedJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
-                AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params(
-                    params, joint_id, rbs1, rbs2, joints,
-                ))
-            }
-            // JointParams::GenericJoint(_) => {
-            //     let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
-            //     AnyJointVelocityConstraint::WGenericConstraint(
-            //         WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
-            //     )
-            // }
-            JointParams::PrismaticJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
-                AnyJointVelocityConstraint::WPrismaticConstraint(
-                    WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
-                )
-            }
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
-                AnyJointVelocityConstraint::WRevoluteConstraint(
-                    WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
-                )
-            }
+        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,
+        );
+
+        for c in out_tmp.into_iter().take(out_tmp_len) {
+            out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
         }
     }
 
     pub fn from_joint_ground<Bodies>(
         params: &IntegrationParameters,
         joint_id: JointIndex,
-        joint: &Joint,
+        joint: &ImpulseJoint,
         bodies: &Bodies,
-    ) -> Self
-    where
+        multibodies: &MultibodyJointSet,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+        out: &mut Vec<Self>,
+    ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyType>
             + ComponentSet<RigidBodyVelocity>
@@ -195,36 +258,102 @@ impl AnyJointVelocityConstraint {
         let status2: &RigidBodyType = bodies.index(handle2.0);
         let flipped = !status2.is_dynamic();
 
-        if flipped {
+        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.index_bundle(handle1.0);
-        let rb2 = bodies.index_bundle(handle2.0);
+        let rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps) =
+            bodies.index_bundle(handle1.0);
+        let rb2: (
+            &RigidBodyPosition,
+            &RigidBodyVelocity,
+            &RigidBodyMassProps,
+            &RigidBodyIds,
+        ) = bodies.index_bundle(handle2.0);
 
-        match &joint.params {
-            JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint(
-                BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
-            ),
-            JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
-                FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
-            ),
-            // JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
-            //     GenericVelocityGroundConstraint::from_params(
-            //         params, joint_id, rb1, rb2, p, flipped,
-            //     ),
-            // ),
-            JointParams::PrismaticJoint(p) => {
-                AnyJointVelocityConstraint::PrismaticGroundConstraint(
-                    PrismaticVelocityGroundConstraint::from_params(
-                        params, joint_id, rb1, rb2, p, flipped,
-                    ),
-                )
+        let (rb_pos1, rb_vel1, rb_mprops1) = rb1;
+        let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2;
+        let frame1 = rb_pos1.position * local_frame1;
+        let frame2 = rb_pos2.position * local_frame2;
+
+        let body1 = SolverBody {
+            linvel: rb_vel1.linvel,
+            angvel: rb_vel1.angvel,
+            im: rb_mprops1.effective_inv_mass,
+            sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
+            world_com: rb_mprops1.world_com,
+            mj_lambda: [crate::INVALID_USIZE],
+        };
+        let body2 = SolverBody {
+            linvel: rb_vel2.linvel,
+            angvel: rb_vel2.angvel,
+            im: rb_mprops2.effective_inv_mass,
+            sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
+            world_com: rb_mprops2.world_com,
+            mj_lambda: [rb_ids2.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 {
+                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,
+            );
+
+            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,
+            );
+
+            for c in out_tmp.into_iter().take(out_tmp_len) {
+                out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
             }
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(p) => RevoluteVelocityGroundConstraint::from_params(
-                params, joint_id, rb1, rb2, p, flipped,
-            ),
         }
     }
 
@@ -232,18 +361,18 @@ impl AnyJointVelocityConstraint {
     pub fn from_wide_joint_ground<Bodies>(
         params: &IntegrationParameters,
         joint_id: [JointIndex; SIMD_WIDTH],
-        joints: [&Joint; SIMD_WIDTH],
+        impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
         bodies: &Bodies,
-    ) -> Self
-    where
+        out: &mut Vec<Self>,
+    ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyType>
             + ComponentSet<RigidBodyVelocity>
             + ComponentSet<RigidBodyMassProps>
             + ComponentSet<RigidBodyIds>,
     {
-        let mut handles1 = gather![|ii| joints[ii].body1];
-        let mut handles2 = gather![|ii| joints[ii].body2];
+        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.index(handles2[ii].0)];
         let mut flipped = [false; SIMD_WIDTH];
 
@@ -254,197 +383,136 @@ impl AnyJointVelocityConstraint {
             }
         }
 
-        let rbs1 = (
+        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.index(handles1[ii].0)],
             gather![|ii| bodies.index(handles1[ii].0)],
             gather![|ii| bodies.index(handles1[ii].0)],
         );
-        let rbs2 = (
+        let rbs2: (
+            [&RigidBodyPosition; SIMD_WIDTH],
+            [&RigidBodyVelocity; SIMD_WIDTH],
+            [&RigidBodyMassProps; SIMD_WIDTH],
+            [&RigidBodyIds; SIMD_WIDTH],
+        ) = (
             gather![|ii| bodies.index(handles2[ii].0)],
             gather![|ii| bodies.index(handles2[ii].0)],
             gather![|ii| bodies.index(handles2[ii].0)],
             gather![|ii| bodies.index(handles2[ii].0)],
         );
 
-        match &joints[0].params {
-            JointParams::BallJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
-                AnyJointVelocityConstraint::WBallGroundConstraint(
-                    WBallVelocityGroundConstraint::from_params(
-                        params, joint_id, rbs1, rbs2, joints, flipped,
-                    ),
-                )
-            }
-            JointParams::FixedJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
-                AnyJointVelocityConstraint::WFixedGroundConstraint(
-                    WFixedVelocityGroundConstraint::from_params(
-                        params, joint_id, rbs1, rbs2, joints, flipped,
-                    ),
-                )
-            }
-            // JointParams::GenericJoint(_) => {
-            //     let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
-            //     AnyJointVelocityConstraint::WGenericGroundConstraint(
-            //         WGenericVelocityGroundConstraint::from_params(
-            //             params, joint_id, rbs1, rbs2, joints, flipped,
-            //         ),
-            //     )
-            // }
-            JointParams::PrismaticJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
-                AnyJointVelocityConstraint::WPrismaticGroundConstraint(
-                    WPrismaticVelocityGroundConstraint::from_params(
-                        params, joint_id, rbs1, rbs2, joints, flipped,
-                    ),
-                )
-            }
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
-                AnyJointVelocityConstraint::WRevoluteGroundConstraint(
-                    WRevoluteVelocityGroundConstraint::from_params(
-                        params, joint_id, rbs1, rbs2, joints, flipped,
-                    ),
-                )
-            }
+        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,
+        );
+
+        for c in out_tmp.into_iter().take(out_tmp_len) {
+            out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    pub fn remove_bias_from_rhs(&mut self) {
         match self {
-            AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas),
-            AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::JointConstraint(c) => c.remove_bias_from_rhs(),
+            AnyJointVelocityConstraint::JointGroundConstraint(c) => c.remove_bias_from_rhs(),
             #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WBallConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::JointConstraintSimd(c) => c.remove_bias_from_rhs(),
             #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.warmstart(mj_lambdas),
-            AnyJointVelocityConstraint::FixedConstraint(c) => c.warmstart(mj_lambdas),
-            AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
-            // AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
-            // AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
-            // #[cfg(feature = "simd-is-enabled")]
-            // AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
-            // #[cfg(feature = "simd-is-enabled")]
-            // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
-            AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
-            AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "dim3")]
-            AnyJointVelocityConstraint::RevoluteConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "dim3")]
-            AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "dim3")]
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "dim3")]
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.warmstart(mj_lambdas),
+            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, mj_lambdas: &mut [DeltaVel<Real>]) {
+    pub fn solve(
+        &mut self,
+        jacobians: &DVector<Real>,
+        mj_lambdas: &mut [DeltaVel<Real>],
+        generic_mj_lambdas: &mut DVector<Real>,
+    ) {
         match self {
-            AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::JointConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::JointGroundConstraint(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WBallConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::JointConstraintSimd(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::FixedConstraint(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
-            // AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
-            // AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
-            // #[cfg(feature = "simd-is-enabled")]
-            // AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
-            // #[cfg(feature = "simd-is-enabled")]
-            // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "dim3")]
-            AnyJointVelocityConstraint::RevoluteConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "dim3")]
-            AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "dim3")]
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "dim3")]
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.solve(mj_lambdas),
+            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::BallConstraint(c) => c.writeback_impulses(joints_all),
-
-            AnyJointVelocityConstraint::BallGroundConstraint(c) => c.writeback_impulses(joints_all),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WBallConstraint(c) => c.writeback_impulses(joints_all),
-
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WBallGroundConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            AnyJointVelocityConstraint::FixedConstraint(c) => c.writeback_impulses(joints_all),
-            AnyJointVelocityConstraint::FixedGroundConstraint(c) => {
+            AnyJointVelocityConstraint::JointConstraint(c) => c.writeback_impulses(joints_all),
+            AnyJointVelocityConstraint::JointGroundConstraint(c) => {
                 c.writeback_impulses(joints_all)
             }
             #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WFixedConstraint(c) => c.writeback_impulses(joints_all),
+            AnyJointVelocityConstraint::JointConstraintSimd(c) => c.writeback_impulses(joints_all),
             #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
+            AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => {
                 c.writeback_impulses(joints_all)
             }
-            // AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
-            // AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
-            //     c.writeback_impulses(joints_all)
-            // }
-            // #[cfg(feature = "simd-is-enabled")]
-            // AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
-            // #[cfg(feature = "simd-is-enabled")]
-            // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
-            //     c.writeback_impulses(joints_all)
-            // }
-            AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
-            AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
+            AnyJointVelocityConstraint::JointGenericConstraint(c) => {
                 c.writeback_impulses(joints_all)
             }
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.writeback_impulses(joints_all),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            #[cfg(feature = "dim3")]
-            AnyJointVelocityConstraint::RevoluteConstraint(c) => c.writeback_impulses(joints_all),
-            #[cfg(feature = "dim3")]
-            AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            #[cfg(feature = "dim3")]
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.writeback_impulses(joints_all),
-            #[cfg(feature = "dim3")]
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => {
+            AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => {
                 c.writeback_impulses(joints_all)
             }
             AnyJointVelocityConstraint::Empty => unreachable!(),

src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs

@@ -0,0 +1,529 @@
+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::{IntegrationParameters, JointData, JointGraphEdge, JointIndex, Multibody};
+use crate::math::{Isometry, Real, DIM};
+use crate::prelude::SPATIAL_DIM;
+use na::{DVector, DVectorSlice, DVectorSliceMut};
+
+#[derive(Debug, Copy, Clone)]
+pub struct JointGenericVelocityConstraint {
+    pub is_rigid_body1: bool,
+    pub is_rigid_body2: bool,
+    pub mj_lambda1: usize,
+    pub mj_lambda2: usize,
+
+    pub ndofs1: usize,
+    pub j_id1: usize,
+    pub ndofs2: usize,
+    pub j_id2: usize,
+
+    pub joint_id: JointIndex,
+
+    pub impulse: Real,
+    pub impulse_bounds: [Real; 2],
+    pub inv_lhs: Real,
+    pub rhs: Real,
+    pub rhs_wo_bias: Real,
+
+    pub writeback_id: WritebackId,
+}
+
+impl JointGenericVelocityConstraint {
+    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,
+            impulse: 0.0,
+            impulse_bounds: [-Real::MAX, Real::MAX],
+            inv_lhs: 0.0,
+            rhs: 0.0,
+            rhs_wo_bias: 0.0,
+            writeback_id: WritebackId::Dof(0),
+        }
+    }
+
+    pub fn lock_axes(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        body2: &SolverBody<Real, 1>,
+        mb1: Option<(&Multibody, usize)>,
+        mb2: Option<(&Multibody, usize)>,
+        frame1: &Isometry<Real>,
+        frame2: &Isometry<Real>,
+        joint: &JointData,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        out: &mut [Self],
+    ) -> usize {
+        let mut len = 0;
+        let locked_axes = joint.locked_axes.bits();
+        let motor_axes = joint.motor_axes.bits();
+        let limit_axes = joint.limit_axes.bits();
+
+        let builder = JointVelocityConstraintBuilder::new(
+            frame1,
+            frame2,
+            &body1.world_com,
+            &body2.world_com,
+            locked_axes,
+        );
+
+        for i in 0..DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_linear_generic(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb1,
+                    mb2,
+                    i,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+        for i in DIM..SPATIAL_DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular_generic(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb1,
+                    mb2,
+                    i - DIM,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in 0..DIM {
+            if motor_axes & (1 << i) != 0 {
+                out[len] = builder.motor_linear_generic(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb1,
+                    mb2,
+                    // locked_ang_axes,
+                    i,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in DIM..SPATIAL_DIM {
+            if (motor_axes >> DIM) & (1 << i) != 0 {
+                out[len] = builder.motor_angular_generic(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb1,
+                    mb2,
+                    i - DIM,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in 0..DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_linear_generic(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb1,
+                    mb2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in DIM..SPATIAL_DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_angular_generic(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb1,
+                    mb2,
+                    i - DIM,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+
+        JointVelocityConstraintBuilder::finalize_generic_constraints(jacobians, &mut out[..len]);
+        len
+    }
+
+    fn wj_id1(&self) -> usize {
+        self.j_id1 + self.ndofs1
+    }
+
+    fn wj_id2(&self) -> usize {
+        self.j_id2 + self.ndofs2
+    }
+
+    fn mj_lambda1<'a>(
+        &self,
+        mj_lambdas: &'a [DeltaVel<Real>],
+        generic_mj_lambdas: &'a DVector<Real>,
+    ) -> DVectorSlice<'a, Real> {
+        if self.is_rigid_body1 {
+            mj_lambdas[self.mj_lambda1].as_vector_slice()
+        } else {
+            generic_mj_lambdas.rows(self.mj_lambda1, self.ndofs1)
+        }
+    }
+
+    fn mj_lambda1_mut<'a>(
+        &self,
+        mj_lambdas: &'a mut [DeltaVel<Real>],
+        generic_mj_lambdas: &'a mut DVector<Real>,
+    ) -> DVectorSliceMut<'a, Real> {
+        if self.is_rigid_body1 {
+            mj_lambdas[self.mj_lambda1].as_vector_slice_mut()
+        } else {
+            generic_mj_lambdas.rows_mut(self.mj_lambda1, self.ndofs1)
+        }
+    }
+
+    fn mj_lambda2<'a>(
+        &self,
+        mj_lambdas: &'a [DeltaVel<Real>],
+        generic_mj_lambdas: &'a DVector<Real>,
+    ) -> DVectorSlice<'a, Real> {
+        if self.is_rigid_body2 {
+            mj_lambdas[self.mj_lambda2].as_vector_slice()
+        } else {
+            generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2)
+        }
+    }
+
+    fn mj_lambda2_mut<'a>(
+        &self,
+        mj_lambdas: &'a mut [DeltaVel<Real>],
+        generic_mj_lambdas: &'a mut DVector<Real>,
+    ) -> DVectorSliceMut<'a, Real> {
+        if self.is_rigid_body2 {
+            mj_lambdas[self.mj_lambda2].as_vector_slice_mut()
+        } else {
+            generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2)
+        }
+    }
+
+    pub fn solve(
+        &mut self,
+        jacobians: &DVector<Real>,
+        mj_lambdas: &mut [DeltaVel<Real>],
+        generic_mj_lambdas: &mut DVector<Real>,
+    ) {
+        let jacobians = jacobians.as_slice();
+
+        let mj_lambda1 = self.mj_lambda1(mj_lambdas, generic_mj_lambdas);
+        let j1 = DVectorSlice::from_slice(&jacobians[self.j_id1..], self.ndofs1);
+        let vel1 = j1.dot(&mj_lambda1);
+
+        let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas);
+        let j2 = DVectorSlice::from_slice(&jacobians[self.j_id2..], self.ndofs2);
+        let vel2 = j2.dot(&mj_lambda2);
+
+        let dvel = self.rhs + (vel2 - vel1);
+        let total_impulse = na::clamp(
+            self.impulse + self.inv_lhs * dvel,
+            self.impulse_bounds[0],
+            self.impulse_bounds[1],
+        );
+        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 wj1 = DVectorSlice::from_slice(&jacobians[self.wj_id1()..], self.ndofs1);
+        mj_lambda1.axpy(delta_impulse, &wj1, 1.0);
+
+        let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas);
+        let wj2 = DVectorSlice::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
+        mj_lambda2.axpy(-delta_impulse, &wj2, 1.0);
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        match self.writeback_id {
+            WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
+            WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
+            WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
+        }
+    }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        self.rhs = self.rhs_wo_bias;
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub struct JointGenericVelocityGroundConstraint {
+    pub mj_lambda2: usize,
+    pub ndofs2: usize,
+    pub j_id2: usize,
+
+    pub joint_id: JointIndex,
+
+    pub impulse: Real,
+    pub impulse_bounds: [Real; 2],
+    pub inv_lhs: Real,
+    pub rhs: Real,
+    pub rhs_wo_bias: Real,
+
+    pub writeback_id: WritebackId,
+}
+
+impl JointGenericVelocityGroundConstraint {
+    pub fn invalid() -> Self {
+        Self {
+            mj_lambda2: crate::INVALID_USIZE,
+            ndofs2: 0,
+            j_id2: crate::INVALID_USIZE,
+            joint_id: 0,
+            impulse: 0.0,
+            impulse_bounds: [-Real::MAX, Real::MAX],
+            inv_lhs: 0.0,
+            rhs: 0.0,
+            rhs_wo_bias: 0.0,
+            writeback_id: WritebackId::Dof(0),
+        }
+    }
+
+    pub fn lock_axes(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        body2: &SolverBody<Real, 1>,
+        mb2: (&Multibody, usize),
+        frame1: &Isometry<Real>,
+        frame2: &Isometry<Real>,
+        joint: &JointData,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        out: &mut [Self],
+    ) -> usize {
+        let mut len = 0;
+        let locked_axes = joint.locked_axes.bits();
+        let motor_axes = joint.motor_axes.bits();
+        let limit_axes = joint.limit_axes.bits();
+
+        let builder = JointVelocityConstraintBuilder::new(
+            frame1,
+            frame2,
+            &body1.world_com,
+            &body2.world_com,
+            locked_axes,
+        );
+
+        for i in 0..DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_linear_generic_ground(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    mb2,
+                    i,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+        for i in DIM..SPATIAL_DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular_generic_ground(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    mb2,
+                    i - DIM,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in 0..DIM {
+            if motor_axes & (1 << i) != 0 {
+                out[len] = builder.motor_linear_generic_ground(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb2,
+                    // locked_ang_axes,
+                    i,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in DIM..SPATIAL_DIM {
+            if (motor_axes >> DIM) & (1 << i) != 0 {
+                out[len] = builder.motor_angular_generic_ground(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb2,
+                    i - DIM,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in 0..DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_linear_generic_ground(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    mb2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in DIM..SPATIAL_DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_angular_generic_ground(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    mb2,
+                    i - DIM,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+
+        JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
+            jacobians,
+            &mut out[..len],
+        );
+        len
+    }
+
+    fn wj_id2(&self) -> usize {
+        self.j_id2 + self.ndofs2
+    }
+
+    fn mj_lambda2<'a>(
+        &self,
+        _mj_lambdas: &'a [DeltaVel<Real>],
+        generic_mj_lambdas: &'a DVector<Real>,
+    ) -> DVectorSlice<'a, Real> {
+        generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2)
+    }
+
+    fn mj_lambda2_mut<'a>(
+        &self,
+        _mj_lambdas: &'a mut [DeltaVel<Real>],
+        generic_mj_lambdas: &'a mut DVector<Real>,
+    ) -> DVectorSliceMut<'a, Real> {
+        generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2)
+    }
+
+    pub fn solve(
+        &mut self,
+        jacobians: &DVector<Real>,
+        mj_lambdas: &mut [DeltaVel<Real>],
+        generic_mj_lambdas: &mut DVector<Real>,
+    ) {
+        let jacobians = jacobians.as_slice();
+
+        let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas);
+        let j2 = DVectorSlice::from_slice(&jacobians[self.j_id2..], self.ndofs2);
+        let vel2 = j2.dot(&mj_lambda2);
+
+        let dvel = self.rhs + vel2;
+        let total_impulse = na::clamp(
+            self.impulse + self.inv_lhs * dvel,
+            self.impulse_bounds[0],
+            self.impulse_bounds[1],
+        );
+        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 wj2 = DVectorSlice::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
+        mj_lambda2.axpy(-delta_impulse, &wj2, 1.0);
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        // FIXME: impulse writeback isn’t supported yet for internal multibody_joint constraints.
+        if self.joint_id != usize::MAX {
+            let joint = &mut joints_all[self.joint_id].weight;
+            match self.writeback_id {
+                WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
+                WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
+                WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
+            }
+        }
+    }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        self.rhs = self.rhs_wo_bias;
+    }
+}

src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs

@@ -0,0 +1,853 @@
+use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{
+    JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
+};
+use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
+use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
+use crate::dynamics::solver::MotorParameters;
+use crate::dynamics::{IntegrationParameters, JointIndex, Multibody};
+use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM};
+use crate::utils::IndexMut2;
+use crate::utils::WDot;
+use na::{DVector, SVector};
+
+#[cfg(feature = "dim3")]
+use crate::utils::WAngularInertia;
+
+impl SolverBody<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)
+            .copy_from(&unit_force);
+        jacobians
+            .fixed_rows_mut::<ANG_DIM>(*j_id + DIM)
+            .copy_from(&unit_torque);
+
+        {
+            let mut out_invm_j = jacobians.fixed_rows_mut::<SPATIAL_DIM>(wj_id);
+            out_invm_j
+                .fixed_rows_mut::<DIM>(0)
+                .axpy(self.im, &unit_force, 0.0);
+
+            #[cfg(feature = "dim2")]
+            {
+                out_invm_j[DIM] *= self.sqrt_ii;
+            }
+            #[cfg(feature = "dim3")]
+            {
+                out_invm_j.fixed_rows_mut::<ANG_DIM>(DIM).gemv(
+                    1.0,
+                    &self.sqrt_ii.into_matrix(),
+                    &unit_torque,
+                    0.0,
+                );
+            }
+        }
+
+        *j_id += SPATIAL_DIM * 2;
+        unit_force.dot(&self.linvel) + unit_torque.gdot(self.angvel)
+    }
+}
+
+impl JointVelocityConstraintBuilder<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>,
+        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 {
+        let is_rigid_body1 = mb1.is_none();
+        let is_rigid_body2 = mb2.is_none();
+
+        let ndofs1 = mb1.map(|(m, _)| m.ndofs()).unwrap_or(SPATIAL_DIM);
+        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
+        } else {
+            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
+        } else {
+            body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians)
+        };
+
+        if is_rigid_body1 {
+            let ang_j_id1 = j_id1 + DIM;
+            let ang_wj_id1 = j_id1 + DIM + ndofs1;
+            let (mut j, wj) = jacobians.rows_range_pair_mut(
+                ang_j_id1..ang_j_id1 + ANG_DIM,
+                ang_wj_id1..ang_wj_id1 + ANG_DIM,
+            );
+            j.copy_from(&wj);
+        }
+
+        if is_rigid_body2 {
+            let ang_j_id2 = j_id2 + DIM;
+            let ang_wj_id2 = j_id2 + DIM + ndofs2;
+            let (mut j, wj) = jacobians.rows_range_pair_mut(
+                ang_j_id2..ang_j_id2 + ANG_DIM,
+                ang_wj_id2..ang_wj_id2 + ANG_DIM,
+            );
+            j.copy_from(&wj);
+        }
+
+        let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
+
+        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]);
+
+        JointGenericVelocityConstraint {
+            is_rigid_body1,
+            is_rigid_body2,
+            mj_lambda1,
+            mj_lambda2,
+            ndofs1,
+            j_id1,
+            ndofs2,
+            j_id2,
+            joint_id,
+            impulse: 0.0,
+            impulse_bounds: [-Real::MAX, Real::MAX],
+            inv_lhs: 0.0,
+            rhs: rhs_wo_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn lock_linear_generic(
+        &self,
+        params: &IntegrationParameters,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        body2: &SolverBody<Real, 1>,
+        mb1: Option<(&Multibody, usize)>,
+        mb2: Option<(&Multibody, usize)>,
+        locked_axis: usize,
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityConstraint {
+        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,
+            body1,
+            body2,
+            mb1,
+            mb2,
+            writeback_id,
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+        );
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
+        c.rhs += rhs_bias;
+        c
+    }
+
+    pub fn limit_linear_generic(
+        &self,
+        params: &IntegrationParameters,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        body2: &SolverBody<Real, 1>,
+        mb1: Option<(&Multibody, usize)>,
+        mb2: Option<(&Multibody, usize)>,
+        limited_axis: usize,
+        limits: [Real; 2],
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityConstraint {
+        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,
+            body1,
+            body2,
+            mb1,
+            mb2,
+            writeback_id,
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+        );
+
+        let dist = self.lin_err.dot(&lin_jac);
+        let min_enabled = dist < limits[0];
+        let max_enabled = limits[1] < dist;
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
+        constraint.rhs += rhs_bias;
+        constraint.impulse_bounds = [
+            min_enabled as u32 as Real * -Real::MAX,
+            max_enabled as u32 as Real * Real::MAX,
+        ];
+
+        constraint
+    }
+
+    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>,
+        mb1: Option<(&Multibody, usize)>,
+        mb2: Option<(&Multibody, usize)>,
+        motor_axis: usize,
+        motor_params: &MotorParameters<Real>,
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityConstraint {
+        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();
+
+        // TODO: do we need the same trick as for the non-generic constraint?
+        // if locked_ang_axes & (1 << motor_axis) != 0 {
+        //     // FIXME: check that this also works for cases
+        //     // whene not all the angular axes are locked.
+        //     constraint.ang_jac1.fill(0.0);
+        //     constraint.ang_jac2.fill(0.0);
+        // }
+
+        let mut constraint = self.lock_jacobians_generic(
+            params,
+            jacobians,
+            j_id,
+            joint_id,
+            body1,
+            body2,
+            mb1,
+            mb2,
+            writeback_id,
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+        );
+
+        let mut rhs_wo_bias = 0.0;
+        if motor_params.stiffness != 0.0 {
+            let dist = self.lin_err.dot(&lin_jac);
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+        }
+
+        if motor_params.damping != 0.0 {
+            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) * motor_params.damping;
+        }
+
+        constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
+        constraint.rhs = rhs_wo_bias;
+        constraint.rhs_wo_bias = rhs_wo_bias;
+        constraint
+    }
+
+    pub fn lock_angular_generic(
+        &self,
+        params: &IntegrationParameters,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        body2: &SolverBody<Real, 1>,
+        mb1: Option<(&Multibody, usize)>,
+        mb2: Option<(&Multibody, usize)>,
+        locked_axis: usize,
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityConstraint {
+        let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+
+        let mut constraint = self.lock_jacobians_generic(
+            params,
+            jacobians,
+            j_id,
+            joint_id,
+            body1,
+            body2,
+            mb1,
+            mb2,
+            writeback_id,
+            na::zero(),
+            ang_jac,
+            ang_jac,
+        );
+
+        let erp_inv_dt = params.erp_inv_dt();
+        #[cfg(feature = "dim2")]
+        let rhs_bias = self.ang_err.im * erp_inv_dt;
+        #[cfg(feature = "dim3")]
+        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+        constraint.rhs += rhs_bias;
+        constraint
+    }
+
+    pub fn limit_angular_generic(
+        &self,
+        params: &IntegrationParameters,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        body2: &SolverBody<Real, 1>,
+        mb1: Option<(&Multibody, usize)>,
+        mb2: Option<(&Multibody, usize)>,
+        limited_axis: usize,
+        limits: [Real; 2],
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityConstraint {
+        let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+
+        let mut constraint = self.lock_jacobians_generic(
+            params,
+            jacobians,
+            j_id,
+            joint_id,
+            body1,
+            body2,
+            mb1,
+            mb2,
+            writeback_id,
+            na::zero(),
+            ang_jac,
+            ang_jac,
+        );
+
+        let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()];
+        #[cfg(feature = "dim2")]
+        let s_ang = self.ang_err.im;
+        #[cfg(feature = "dim3")]
+        let s_ang = self.ang_err.imag()[limited_axis];
+        let min_enabled = s_ang < s_limits[0];
+        let max_enabled = s_limits[1] < s_ang;
+        let impulse_bounds = [
+            min_enabled as u32 as Real * -Real::MAX,
+            max_enabled as u32 as Real * Real::MAX,
+        ];
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias =
+            ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
+
+        constraint.rhs += rhs_bias;
+        constraint.impulse_bounds = impulse_bounds;
+        constraint
+    }
+
+    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>,
+        mb1: Option<(&Multibody, usize)>,
+        mb2: Option<(&Multibody, usize)>,
+        _motor_axis: usize,
+        motor_params: &MotorParameters<Real>,
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityConstraint {
+        // let mut ang_jac = self.ang_basis.column(motor_axis).into_owned();
+        #[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,
+            body1,
+            body2,
+            mb1,
+            mb2,
+            writeback_id,
+            na::zero(),
+            ang_jac,
+            ang_jac,
+        );
+
+        let mut rhs_wo_bias = 0.0;
+        if motor_params.stiffness != 0.0 {
+            #[cfg(feature = "dim2")]
+            let s_ang_dist = self.ang_err.im;
+            #[cfg(feature = "dim3")]
+            let s_ang_dist = self.ang_err.imag()[_motor_axis];
+            let s_target_ang = motor_params.target_pos.sin();
+            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+        }
+
+        if motor_params.damping != 0.0 {
+            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+            rhs_wo_bias += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping;
+        }
+
+        constraint.rhs_wo_bias = rhs_wo_bias;
+        constraint.rhs = rhs_wo_bias;
+        constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
+        constraint
+    }
+
+    pub fn finalize_generic_constraints(
+        jacobians: &mut DVector<Real>,
+        constraints: &mut [JointGenericVelocityConstraint],
+    ) {
+        // TODO: orthogonalization doesn’t seem to give good results for multibodies?
+        const ORTHOGONALIZE: bool = false;
+        let len = constraints.len();
+        let ndofs1 = constraints[0].ndofs1;
+        let ndofs2 = constraints[0].ndofs2;
+
+        // Use the modified Gramm-Schmidt orthogonalization.
+        for j in 0..len {
+            let c_j = &mut constraints[j];
+
+            let jac_j1 = jacobians.rows(c_j.j_id1, ndofs1);
+            let jac_j2 = jacobians.rows(c_j.j_id2, ndofs2);
+            let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1);
+            let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
+
+            let dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2);
+            let inv_dot_jj = crate::utils::inv(dot_jj);
+            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+
+            if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
+                // Don't remove constraints with limited forces from the others
+                // because they may not deliver the necessary forces to fulfill
+                // the removed parts of other constraints.
+                continue;
+            }
+
+            if !ORTHOGONALIZE {
+                continue;
+            }
+
+            for i in (j + 1)..len {
+                let (c_i, c_j) = constraints.index_mut_const(i, j);
+
+                let jac_i1 = jacobians.rows(c_i.j_id1, ndofs1);
+                let jac_i2 = jacobians.rows(c_i.j_id2, ndofs2);
+                let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1);
+                let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
+
+                let dot_ij = jac_i1.dot(&w_jac_j1) + jac_i2.dot(&w_jac_j2);
+                let coeff = dot_ij * inv_dot_jj;
+
+                let (mut jac_i, jac_j) = jacobians.rows_range_pair_mut(
+                    c_i.j_id1..c_i.j_id1 + 2 * (ndofs1 + ndofs2),
+                    c_j.j_id1..c_j.j_id1 + 2 * (ndofs1 + ndofs2),
+                );
+
+                jac_i.axpy(-coeff, &jac_j, 1.0);
+
+                c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
+                c_i.rhs -= c_j.rhs * coeff;
+            }
+        }
+    }
+}
+
+impl JointVelocityConstraintBuilder<Real> {
+    pub fn lock_jacobians_generic_ground(
+        &self,
+        params: &IntegrationParameters,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        (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 {
+        let ndofs2 = mb2.ndofs();
+
+        let 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) * params.velocity_solve_fraction;
+
+        let mj_lambda2 = mb2.solver_id;
+
+        JointGenericVelocityGroundConstraint {
+            mj_lambda2,
+            ndofs2,
+            j_id2,
+            joint_id,
+            impulse: 0.0,
+            impulse_bounds: [-Real::MAX, Real::MAX],
+            inv_lhs: 0.0,
+            rhs: rhs_wo_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn lock_linear_generic_ground(
+        &self,
+        params: &IntegrationParameters,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        mb2: (&Multibody, usize),
+        locked_axis: usize,
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityGroundConstraint {
+        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,
+            jacobians,
+            j_id,
+            joint_id,
+            body1,
+            mb2,
+            writeback_id,
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+        );
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
+        c.rhs += rhs_bias;
+        c
+    }
+
+    pub fn limit_linear_generic_ground(
+        &self,
+        params: &IntegrationParameters,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        mb2: (&Multibody, usize),
+        limited_axis: usize,
+        limits: [Real; 2],
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityGroundConstraint {
+        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,
+            jacobians,
+            j_id,
+            joint_id,
+            body1,
+            mb2,
+            writeback_id,
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+        );
+
+        let dist = self.lin_err.dot(&lin_jac);
+        let min_enabled = dist < limits[0];
+        let max_enabled = limits[1] < dist;
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
+        constraint.rhs += rhs_bias;
+        constraint.impulse_bounds = [
+            min_enabled as u32 as Real * -Real::MAX,
+            max_enabled as u32 as Real * Real::MAX,
+        ];
+
+        constraint
+    }
+
+    pub fn motor_linear_generic_ground(
+        &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.
+        mb2: (&Multibody, usize),
+        motor_axis: usize,
+        motor_params: &MotorParameters<Real>,
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityGroundConstraint {
+        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();
+
+        // TODO: do we need the same trick as for the non-generic constraint?
+        // if locked_ang_axes & (1 << motor_axis) != 0 {
+        //     // FIXME: check that this also works for cases
+        //     // whene not all the angular axes are locked.
+        //     constraint.ang_jac1.fill(0.0);
+        //     constraint.ang_jac2.fill(0.0);
+        // }
+
+        let mut constraint = self.lock_jacobians_generic_ground(
+            params,
+            jacobians,
+            j_id,
+            joint_id,
+            body1,
+            mb2,
+            writeback_id,
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+        );
+
+        let mut rhs_wo_bias = 0.0;
+        if motor_params.stiffness != 0.0 {
+            let dist = self.lin_err.dot(&lin_jac);
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+        }
+
+        if motor_params.damping != 0.0 {
+            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) * motor_params.damping;
+        }
+
+        constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
+        constraint.rhs = rhs_wo_bias;
+        constraint.rhs_wo_bias = rhs_wo_bias;
+        constraint
+    }
+
+    pub fn lock_angular_generic_ground(
+        &self,
+        params: &IntegrationParameters,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        mb2: (&Multibody, usize),
+        locked_axis: usize,
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityGroundConstraint {
+        let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+
+        let mut constraint = self.lock_jacobians_generic_ground(
+            params,
+            jacobians,
+            j_id,
+            joint_id,
+            body1,
+            mb2,
+            writeback_id,
+            na::zero(),
+            ang_jac,
+            ang_jac,
+        );
+
+        let erp_inv_dt = params.erp_inv_dt();
+        #[cfg(feature = "dim2")]
+        let rhs_bias = self.ang_err.im * erp_inv_dt;
+        #[cfg(feature = "dim3")]
+        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+        constraint.rhs += rhs_bias;
+        constraint
+    }
+
+    pub fn limit_angular_generic_ground(
+        &self,
+        params: &IntegrationParameters,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        mb2: (&Multibody, usize),
+        limited_axis: usize,
+        limits: [Real; 2],
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityGroundConstraint {
+        let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+
+        let mut constraint = self.lock_jacobians_generic_ground(
+            params,
+            jacobians,
+            j_id,
+            joint_id,
+            body1,
+            mb2,
+            writeback_id,
+            na::zero(),
+            ang_jac,
+            ang_jac,
+        );
+
+        let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()];
+        #[cfg(feature = "dim2")]
+        let s_ang = self.ang_err.im;
+        #[cfg(feature = "dim3")]
+        let s_ang = self.ang_err.imag()[limited_axis];
+        let min_enabled = s_ang < s_limits[0];
+        let max_enabled = s_limits[1] < s_ang;
+        let impulse_bounds = [
+            min_enabled as u32 as Real * -Real::MAX,
+            max_enabled as u32 as Real * Real::MAX,
+        ];
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias =
+            ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
+
+        constraint.rhs += rhs_bias;
+        constraint.impulse_bounds = impulse_bounds;
+        constraint
+    }
+
+    pub fn motor_angular_generic_ground(
+        &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.
+        mb2: (&Multibody, usize),
+        _motor_axis: usize,
+        motor_params: &MotorParameters<Real>,
+        writeback_id: WritebackId,
+    ) -> JointGenericVelocityGroundConstraint {
+        // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
+        #[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,
+            jacobians,
+            j_id,
+            joint_id,
+            body1,
+            mb2,
+            writeback_id,
+            na::zero(),
+            ang_jac,
+            ang_jac,
+        );
+
+        let mut rhs = 0.0;
+        if motor_params.stiffness != 0.0 {
+            #[cfg(feature = "dim2")]
+            let s_ang_dist = self.ang_err.im;
+            #[cfg(feature = "dim3")]
+            let s_ang_dist = self.ang_err.imag()[_motor_axis];
+            let s_target_ang = motor_params.target_pos.sin();
+            rhs += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+        }
+
+        if motor_params.damping != 0.0 {
+            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+            rhs += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping;
+        }
+
+        constraint.rhs_wo_bias = rhs;
+        constraint.rhs = rhs;
+        constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
+        constraint
+    }
+
+    pub fn finalize_generic_constraints_ground(
+        jacobians: &mut DVector<Real>,
+        constraints: &mut [JointGenericVelocityGroundConstraint],
+    ) {
+        // TODO: orthogonalization doesn’t seem to give good results for multibodies?
+        const ORTHOGONALIZE: bool = false;
+        let len = constraints.len();
+        let ndofs2 = constraints[0].ndofs2;
+
+        // Use the modified Gramm-Schmidt orthogonalization.
+        for j in 0..len {
+            let c_j = &mut constraints[j];
+
+            let jac_j2 = jacobians.rows(c_j.j_id2, ndofs2);
+            let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
+
+            let dot_jj = jac_j2.dot(&w_jac_j2);
+            let inv_dot_jj = crate::utils::inv(dot_jj);
+            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+
+            if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
+                // Don't remove constraints with limited forces from the others
+                // because they may not deliver the necessary forces to fulfill
+                // the removed parts of other constraints.
+                continue;
+            }
+
+            if !ORTHOGONALIZE {
+                continue;
+            }
+
+            for i in (j + 1)..len {
+                let (c_i, c_j) = constraints.index_mut_const(i, j);
+
+                let jac_i2 = jacobians.rows(c_i.j_id2, ndofs2);
+                let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
+
+                let dot_ij = jac_i2.dot(&w_jac_j2);
+                let coeff = dot_ij * inv_dot_jj;
+
+                let (mut jac_i, jac_j) = jacobians.rows_range_pair_mut(
+                    c_i.j_id2..c_i.j_id2 + 2 * ndofs2,
+                    c_j.j_id2..c_j.j_id2 + 2 * ndofs2,
+                );
+
+                jac_i.axpy(-coeff, &jac_j, 1.0);
+
+                c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
+                c_i.rhs -= c_j.rhs * coeff;
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/joint_position_constraint.rs

@@ -1,280 +0,0 @@
-use super::{
-    BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
-    FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
-};
-#[cfg(feature = "dim3")]
-use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
-#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
-use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint};
-
-#[cfg(feature = "simd-is-enabled")]
-use super::{
-    WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint,
-    WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
-    WPrismaticPositionGroundConstraint,
-};
-use crate::data::{BundleSet, ComponentSet};
-use crate::dynamics::{
-    IntegrationParameters, Joint, JointParams, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-    RigidBodyType,
-};
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::SIMD_WIDTH;
-use crate::math::{Isometry, Real};
-
-pub(crate) enum AnyJointPositionConstraint {
-    BallJoint(BallPositionConstraint),
-    BallGroundConstraint(BallPositionGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WBallJoint(WBallPositionConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WBallGroundConstraint(WBallPositionGroundConstraint),
-    FixedJoint(FixedPositionConstraint),
-    FixedGroundConstraint(FixedPositionGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WFixedJoint(WFixedPositionConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WFixedGroundConstraint(WFixedPositionGroundConstraint),
-    // GenericJoint(GenericPositionConstraint),
-    // GenericGroundConstraint(GenericPositionGroundConstraint),
-    // #[cfg(feature = "simd-is-enabled")]
-    // WGenericJoint(WGenericPositionConstraint),
-    // #[cfg(feature = "simd-is-enabled")]
-    // WGenericGroundConstraint(WGenericPositionGroundConstraint),
-    PrismaticJoint(PrismaticPositionConstraint),
-    PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WPrismaticJoint(WPrismaticPositionConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WPrismaticGroundConstraint(WPrismaticPositionGroundConstraint),
-    #[cfg(feature = "dim3")]
-    RevoluteJoint(RevolutePositionConstraint),
-    #[cfg(feature = "dim3")]
-    RevoluteGroundConstraint(RevolutePositionGroundConstraint),
-    #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
-    WRevoluteJoint(WRevolutePositionConstraint),
-    #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
-    WRevoluteGroundConstraint(WRevolutePositionGroundConstraint),
-    #[allow(dead_code)] // The Empty variant is only used with parallel code.
-    Empty,
-}
-
-impl AnyJointPositionConstraint {
-    pub fn from_joint<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
-    where
-        Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
-    {
-        let rb1 = bodies.index_bundle(joint.body1.0);
-        let rb2 = bodies.index_bundle(joint.body2.0);
-
-        match &joint.params {
-            JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint(
-                BallPositionConstraint::from_params(rb1, rb2, p),
-            ),
-            JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
-                FixedPositionConstraint::from_params(rb1, rb2, p),
-            ),
-            // JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
-            //     GenericPositionConstraint::from_params(rb1, rb2, p),
-            // ),
-            JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
-                PrismaticPositionConstraint::from_params(rb1, rb2, p),
-            ),
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteJoint(
-                RevolutePositionConstraint::from_params(rb1, rb2, p),
-            ),
-        }
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
-    where
-        Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
-    {
-        let rbs1 = (
-            gather![|ii| bodies.index(joints[ii].body1.0)],
-            gather![|ii| bodies.index(joints[ii].body1.0)],
-        );
-        let rbs2 = (
-            gather![|ii| bodies.index(joints[ii].body2.0)],
-            gather![|ii| bodies.index(joints[ii].body2.0)],
-        );
-
-        match &joints[0].params {
-            JointParams::BallJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
-                AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
-                    rbs1, rbs2, joints,
-                ))
-            }
-            JointParams::FixedJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
-                AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params(
-                    rbs1, rbs2, joints,
-                ))
-            }
-            // JointParams::GenericJoint(_) => {
-            //     let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
-            //     AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
-            //         rbs1, rbs2, joints,
-            //     ))
-            // }
-            JointParams::PrismaticJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
-                AnyJointPositionConstraint::WPrismaticJoint(
-                    WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints),
-                )
-            }
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
-                AnyJointPositionConstraint::WRevoluteJoint(
-                    WRevolutePositionConstraint::from_params(rbs1, rbs2, joints),
-                )
-            }
-        }
-    }
-
-    pub fn from_joint_ground<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
-    where
-        Bodies: ComponentSet<RigidBodyType>
-            + ComponentSet<RigidBodyPosition>
-            + ComponentSet<RigidBodyMassProps>
-            + ComponentSet<RigidBodyIds>,
-    {
-        let mut handle1 = joint.body1;
-        let mut handle2 = joint.body2;
-
-        let status2: &RigidBodyType = bodies.index(handle2.0);
-        let flipped = !status2.is_dynamic();
-
-        if flipped {
-            std::mem::swap(&mut handle1, &mut handle2);
-        }
-
-        let rb1 = bodies.index(handle1.0);
-        let rb2 = (bodies.index(handle2.0), bodies.index(handle2.0));
-
-        match &joint.params {
-            JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint(
-                BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
-            ),
-            JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
-                FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
-            ),
-            // JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
-            //     GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
-            // ),
-            JointParams::PrismaticJoint(p) => {
-                AnyJointPositionConstraint::PrismaticGroundConstraint(
-                    PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
-                )
-            }
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteGroundConstraint(
-                RevolutePositionGroundConstraint::from_params(rb1, rb2, p, flipped),
-            ),
-        }
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint_ground<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
-    where
-        Bodies: ComponentSet<RigidBodyType>
-            + ComponentSet<RigidBodyPosition>
-            + ComponentSet<RigidBodyMassProps>
-            + ComponentSet<RigidBodyIds>,
-    {
-        let mut handles1 = gather![|ii| joints[ii].body1];
-        let mut handles2 = gather![|ii| joints[ii].body2];
-        let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
-
-        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 rbs1 = gather![|ii| bodies.index(handles1[ii].0)];
-        let rbs2 = (
-            gather![|ii| bodies.index(handles2[ii].0)],
-            gather![|ii| bodies.index(handles2[ii].0)],
-        );
-
-        match &joints[0].params {
-            JointParams::BallJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
-                AnyJointPositionConstraint::WBallGroundConstraint(
-                    WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
-                )
-            }
-            JointParams::FixedJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
-                AnyJointPositionConstraint::WFixedGroundConstraint(
-                    WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
-                )
-            }
-            // JointParams::GenericJoint(_) => {
-            //     let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
-            //     AnyJointPositionConstraint::WGenericGroundConstraint(
-            //         WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
-            //     )
-            // }
-            JointParams::PrismaticJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
-                AnyJointPositionConstraint::WPrismaticGroundConstraint(
-                    WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
-                )
-            }
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(_) => {
-                let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
-                AnyJointPositionConstraint::WRevoluteGroundConstraint(
-                    WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
-                )
-            }
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        match self {
-            AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions),
-            AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointPositionConstraint::WBallJoint(c) => c.solve(params, positions),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointPositionConstraint::WBallGroundConstraint(c) => c.solve(params, positions),
-            AnyJointPositionConstraint::FixedJoint(c) => c.solve(params, positions),
-            AnyJointPositionConstraint::FixedGroundConstraint(c) => c.solve(params, positions),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
-            // AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
-            // AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
-            // #[cfg(feature = "simd-is-enabled")]
-            // AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
-            // #[cfg(feature = "simd-is-enabled")]
-            // AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
-            AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
-            AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointPositionConstraint::WPrismaticJoint(c) => c.solve(params, positions),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointPositionConstraint::WPrismaticGroundConstraint(c) => c.solve(params, positions),
-            #[cfg(feature = "dim3")]
-            AnyJointPositionConstraint::RevoluteJoint(c) => c.solve(params, positions),
-            #[cfg(feature = "dim3")]
-            AnyJointPositionConstraint::RevoluteGroundConstraint(c) => c.solve(params, positions),
-            #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
-            AnyJointPositionConstraint::WRevoluteJoint(c) => c.solve(params, positions),
-            #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
-            AnyJointPositionConstraint::WRevoluteGroundConstraint(c) => c.solve(params, positions),
-            AnyJointPositionConstraint::Empty => unreachable!(),
-        }
-    }
-}

src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs

@@ -0,0 +1,608 @@
+use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder;
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex};
+use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
+use crate::utils::{WDot, WReal};
+use simba::simd::SimdRealField;
+
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::math::{SimdReal, SIMD_WIDTH},
+    na::SimdValue,
+};
+
+#[derive(Copy, Clone, PartialEq, Debug)]
+pub struct MotorParameters<N: SimdRealField> {
+    pub stiffness: N,
+    pub damping: N,
+    pub gamma: N,
+    // pub keep_lhs: bool,
+    pub target_pos: N,
+    pub target_vel: N,
+    pub max_impulse: N,
+}
+
+impl<N: SimdRealField> Default for MotorParameters<N> {
+    fn default() -> Self {
+        Self {
+            stiffness: N::zero(),
+            damping: N::zero(),
+            gamma: N::zero(),
+            // keep_lhs: true,
+            target_pos: N::zero(),
+            target_vel: N::zero(),
+            max_impulse: N::zero(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, PartialEq, Eq, Debug)]
+pub enum WritebackId {
+    Dof(usize),
+    Limit(usize),
+    Motor(usize),
+}
+
+// TODO: right now we only use this for impulse_joints.
+// However, it may actually be a good idea to use this everywhere in
+// the solver, to avoid fetching data from the rigid-body set
+// every time.
+#[derive(Copy, Clone)]
+pub struct SolverBody<N: SimdRealField, const LANES: usize> {
+    pub linvel: Vector<N>,
+    pub angvel: AngVector<N>,
+    pub im: N,
+    pub sqrt_ii: AngularInertia<N>,
+    pub world_com: Point<N>,
+    pub mj_lambda: [usize; LANES],
+}
+
+#[derive(Debug, Copy, Clone)]
+pub struct JointVelocityConstraint<N: SimdRealField, const LANES: usize> {
+    pub mj_lambda1: [usize; LANES],
+    pub mj_lambda2: [usize; LANES],
+
+    pub joint_id: [JointIndex; LANES],
+
+    pub impulse: N,
+    pub impulse_bounds: [N; 2],
+    pub lin_jac: Vector<N>,
+    pub ang_jac1: AngVector<N>,
+    pub ang_jac2: AngVector<N>,
+
+    pub inv_lhs: N,
+    pub rhs: N,
+    pub rhs_wo_bias: N,
+
+    pub im1: N,
+    pub im2: N,
+
+    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(),
+            rhs: N::zero(),
+            rhs_wo_bias: N::zero(),
+            im1: N::zero(),
+            im2: N::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));
+        let dangvel =
+            self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular);
+
+        let rhs = dlinvel + dangvel + self.rhs;
+        let total_impulse = (self.impulse + self.inv_lhs * rhs)
+            .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
+        let delta_impulse = total_impulse - self.impulse;
+        self.impulse = total_impulse;
+
+        let lin_impulse = self.lin_jac * delta_impulse;
+        let ang_impulse1 = self.ang_jac1 * delta_impulse;
+        let ang_impulse2 = self.ang_jac2 * delta_impulse;
+
+        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.angular += ang_impulse1;
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= ang_impulse2;
+    }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        self.rhs = self.rhs_wo_bias;
+    }
+}
+
+impl JointVelocityConstraint<Real, 1> {
+    pub fn lock_axes(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        body2: &SolverBody<Real, 1>,
+        frame1: &Isometry<Real>,
+        frame2: &Isometry<Real>,
+        joint: &JointData,
+        out: &mut [Self],
+    ) -> usize {
+        let mut len = 0;
+        let locked_axes = joint.locked_axes.bits();
+        let motor_axes = joint.motor_axes.bits();
+        let limit_axes = joint.limit_axes.bits();
+
+        let builder = JointVelocityConstraintBuilder::new(
+            frame1,
+            frame2,
+            &body1.world_com,
+            &body2.world_com,
+            locked_axes,
+        );
+
+        for i in 0..DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] =
+                    builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i));
+                len += 1;
+            }
+        }
+        for i in DIM..SPATIAL_DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i - DIM,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in 0..DIM {
+            if motor_axes & (1 << i) != 0 {
+                out[len] = builder.motor_linear(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    locked_axes >> DIM,
+                    i,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
+        for i in DIM..SPATIAL_DIM {
+            if motor_axes & (1 << i) != 0 {
+                out[len] = builder.motor_angular(
+                    [joint_id],
+                    body1,
+                    body2,
+                    i - DIM,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in 0..DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_linear(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+        for i in DIM..SPATIAL_DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_angular(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i - DIM,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+
+        JointVelocityConstraintBuilder::finalize_constraints(&mut out[..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];
+
+        self.solve_generic(&mut mj_lambda1, &mut mj_lambda2);
+
+        mj_lambdas[self.mj_lambda1[0] as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2[0] as usize] = mj_lambda2;
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id[0]].weight;
+        match self.writeback_id {
+            WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
+            WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
+            WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
+        }
+    }
+}
+#[cfg(feature = "simd-is-enabled")]
+impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
+    pub fn lock_axes(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        body1: &SolverBody<SimdReal, SIMD_WIDTH>,
+        body2: &SolverBody<SimdReal, SIMD_WIDTH>,
+        frame1: &Isometry<SimdReal>,
+        frame2: &Isometry<SimdReal>,
+        locked_axes: u8,
+        out: &mut [Self],
+    ) -> usize {
+        let builder = JointVelocityConstraintBuilder::new(
+            frame1,
+            frame2,
+            &body1.world_com,
+            &body2.world_com,
+            locked_axes,
+        );
+
+        let mut len = 0;
+        for i in 0..DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] =
+                    builder.lock_linear(params, joint_id, body1, body2, i, WritebackId::Dof(i));
+                len += 1;
+            }
+        }
+
+        for i in DIM..SPATIAL_DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular(
+                    params,
+                    joint_id,
+                    body1,
+                    body2,
+                    i - DIM,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+
+        JointVelocityConstraintBuilder::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]),
+            angular: AngVector::from(gather![
+                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
+            ]),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
+            angular: AngVector::from(gather![
+                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
+            ]),
+        };
+
+        self.solve_generic(&mut mj_lambda1, &mut mj_lambda2);
+
+        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);
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let impulses: [_; SIMD_WIDTH] = self.impulse.into();
+
+        // TODO: should we move the iteration on ii deeper in the mested match?
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            match self.writeback_id {
+                WritebackId::Dof(i) => joint.impulses[i] = impulses[ii],
+                WritebackId::Limit(i) => joint.data.limits[i].impulse = impulses[ii],
+                WritebackId::Motor(i) => joint.data.motors[i].impulse = impulses[ii],
+            }
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub struct JointVelocityGroundConstraint<N: SimdRealField, const LANES: usize> {
+    pub mj_lambda2: [usize; LANES],
+
+    pub joint_id: [JointIndex; LANES],
+
+    pub impulse: N,
+    pub impulse_bounds: [N; 2],
+    pub lin_jac: Vector<N>,
+    pub ang_jac2: AngVector<N>,
+
+    pub inv_lhs: N,
+    pub rhs: N,
+    pub rhs_wo_bias: N,
+
+    pub im2: N,
+
+    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(),
+            rhs: N::zero(),
+            rhs_wo_bias: N::zero(),
+            im2: N::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;
+
+        let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs;
+        let total_impulse = (self.impulse + self.inv_lhs * dvel)
+            .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
+        let delta_impulse = total_impulse - self.impulse;
+        self.impulse = total_impulse;
+
+        let lin_impulse = self.lin_jac * delta_impulse;
+        let ang_impulse = self.ang_jac2 * delta_impulse;
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= ang_impulse;
+    }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        self.rhs = self.rhs_wo_bias;
+    }
+}
+
+impl JointVelocityGroundConstraint<Real, 1> {
+    pub fn lock_axes(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        body1: &SolverBody<Real, 1>,
+        body2: &SolverBody<Real, 1>,
+        frame1: &Isometry<Real>,
+        frame2: &Isometry<Real>,
+        joint: &JointData,
+        out: &mut [Self],
+    ) -> usize {
+        let mut len = 0;
+        let locked_axes = joint.locked_axes.bits() as u8;
+        let motor_axes = joint.motor_axes.bits() as u8;
+        let limit_axes = joint.limit_axes.bits() as u8;
+
+        let builder = JointVelocityConstraintBuilder::new(
+            frame1,
+            frame2,
+            &body1.world_com,
+            &body2.world_com,
+            locked_axes,
+        );
+
+        for i in 0..DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_linear_ground(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+        for i in DIM..SPATIAL_DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular_ground(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i - DIM,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in 0..DIM {
+            if motor_axes & (1 << i) != 0 {
+                out[len] = builder.motor_linear_ground(
+                    [joint_id],
+                    body1,
+                    body2,
+                    i,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
+        for i in DIM..SPATIAL_DIM {
+            if motor_axes & (1 << i) != 0 {
+                out[len] = builder.motor_angular_ground(
+                    [joint_id],
+                    body1,
+                    body2,
+                    i - DIM,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
+
+        for i in 0..DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_linear_ground(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+        for i in DIM..SPATIAL_DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_angular_ground(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i - DIM,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+
+        JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..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 writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id[0]].weight;
+        match self.writeback_id {
+            WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
+            WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
+            WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
+        }
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
+    pub fn lock_axes(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        body1: &SolverBody<SimdReal, SIMD_WIDTH>,
+        body2: &SolverBody<SimdReal, SIMD_WIDTH>,
+        frame1: &Isometry<SimdReal>,
+        frame2: &Isometry<SimdReal>,
+        locked_axes: u8,
+        out: &mut [Self],
+    ) -> usize {
+        let mut len = 0;
+        let builder = JointVelocityConstraintBuilder::new(
+            frame1,
+            frame2,
+            &body1.world_com,
+            &body2.world_com,
+            locked_axes,
+        );
+
+        for i in 0..DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_linear_ground(
+                    params,
+                    joint_id,
+                    body1,
+                    body2,
+                    i,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+        for i in DIM..SPATIAL_DIM {
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular_ground(
+                    params,
+                    joint_id,
+                    body1,
+                    body2,
+                    i - DIM,
+                    WritebackId::Dof(i),
+                );
+                len += 1;
+            }
+        }
+
+        JointVelocityConstraintBuilder::finalize_ground_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]),
+            angular: AngVector::from(gather![
+                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
+            ]),
+        };
+
+        self.solve_generic(&mut mj_lambda2);
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let impulses: [_; SIMD_WIDTH] = self.impulse.into();
+
+        // TODO: should we move the iteration on ii deeper in the mested match?
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            match self.writeback_id {
+                WritebackId::Dof(i) => joint.impulses[i] = impulses[ii],
+                WritebackId::Limit(i) => joint.data.limits[i].impulse = impulses[ii],
+                WritebackId::Motor(i) => joint.data.motors[i].impulse = impulses[ii],
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs

@@ -0,0 +1,699 @@
+use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
+    JointVelocityConstraint, JointVelocityGroundConstraint, WritebackId,
+};
+use crate::dynamics::solver::joint_constraint::SolverBody;
+use crate::dynamics::solver::MotorParameters;
+use crate::dynamics::{IntegrationParameters, JointIndex};
+use crate::math::{Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
+use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
+use na::SMatrix;
+use simba::simd::SimdRealField;
+
+#[derive(Debug, Copy, Clone)]
+pub struct JointVelocityConstraintBuilder<N: SimdRealField> {
+    pub basis: Matrix<N>,
+    pub cmat1_basis: SMatrix<N, ANG_DIM, DIM>,
+    pub cmat2_basis: SMatrix<N, ANG_DIM, DIM>,
+    pub ang_basis: SMatrix<N, ANG_DIM, ANG_DIM>,
+    pub lin_err: Vector<N>,
+    pub ang_err: Rotation<N>,
+}
+
+impl<N: WReal> JointVelocityConstraintBuilder<N> {
+    pub fn new(
+        frame1: &Isometry<N>,
+        frame2: &Isometry<N>,
+        world_com1: &Point<N>,
+        world_com2: &Point<N>,
+        locked_lin_axes: u8,
+    ) -> Self {
+        let mut frame1 = *frame1;
+        let basis = frame1.rotation.to_rotation_matrix().into_inner();
+        let lin_err = frame2.translation.vector - frame1.translation.vector;
+
+        // Adjust the point of application of the force for the first body,
+        // by snapping free axes to the second frame’s center (to account for
+        // the allowed relative movement).
+        {
+            let mut new_center1 = frame2.translation.vector; // First, assume all dofs are free.
+
+            // Then snap the locked ones.
+            for i in 0..DIM {
+                if locked_lin_axes & (1 << i) != 0 {
+                    let axis = basis.column(i);
+                    new_center1 -= axis * lin_err.dot(&axis);
+                }
+            }
+            frame1.translation.vector = new_center1;
+        }
+
+        let r1 = frame1.translation.vector - world_com1.coords;
+        let r2 = frame2.translation.vector - world_com2.coords;
+
+        let cmat1 = r1.gcross_matrix();
+        let cmat2 = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // The mut is needed for 3D
+        let mut ang_basis = frame1.rotation.diff_conj1_2(&frame2.rotation).transpose();
+        #[allow(unused_mut)] // The mut is needed for 3D
+        let mut ang_err = frame1.rotation.inverse() * frame2.rotation;
+
+        #[cfg(feature = "dim3")]
+        {
+            let sgn = N::one().simd_copysign(frame1.rotation.dot(&frame2.rotation));
+            ang_basis *= sgn;
+            *ang_err.as_mut_unchecked() *= sgn;
+        }
+
+        Self {
+            basis,
+            cmat1_basis: cmat1 * basis,
+            cmat2_basis: cmat2 * basis,
+            ang_basis,
+            lin_err,
+            ang_err,
+        }
+    }
+
+    pub fn limit_linear<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        limited_axis: usize,
+        limits: [N; 2],
+        writeback_id: WritebackId,
+    ) -> JointVelocityConstraint<N, LANES> {
+        let zero = N::zero();
+        let mut constraint =
+            self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id);
+
+        let dist = self.lin_err.dot(&constraint.lin_jac);
+        let min_enabled = dist.simd_lt(limits[0]);
+        let max_enabled = limits[1].simd_lt(dist);
+
+        let erp_inv_dt = N::splat(params.erp_inv_dt());
+        let rhs_bias =
+            ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
+        constraint.rhs = constraint.rhs_wo_bias + rhs_bias;
+        constraint.impulse_bounds = [
+            N::splat(-Real::INFINITY).select(min_enabled, zero),
+            N::splat(Real::INFINITY).select(max_enabled, zero),
+        ];
+
+        constraint
+    }
+
+    pub fn motor_linear<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        _locked_ang_axes: u8,
+        motor_axis: usize,
+        motor_params: &MotorParameters<N>,
+        writeback_id: WritebackId,
+    ) -> JointVelocityConstraint<N, LANES> {
+        let mut constraint =
+            self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id);
+
+        // if locked_ang_axes & (1 << motor_axis) != 0 {
+        //     // FIXME: check that this also works for cases
+        //     // when not all the angular axes are locked.
+        //     constraint.ang_jac1 = na::zero();
+        //     constraint.ang_jac2 = na::zero();
+        // }
+
+        let mut rhs_wo_bias = N::zero();
+        if motor_params.stiffness != N::zero() {
+            let dist = self.lin_err.dot(&constraint.lin_jac);
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+        }
+
+        if motor_params.damping != N::zero() {
+            let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel))
+                + (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel));
+            rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
+        }
+
+        constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
+        constraint.rhs = rhs_wo_bias;
+        constraint.rhs_wo_bias = rhs_wo_bias;
+        constraint
+    }
+
+    pub fn lock_linear<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        locked_axis: usize,
+        writeback_id: WritebackId,
+    ) -> JointVelocityConstraint<N, LANES> {
+        let lin_jac = self.basis.column(locked_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let mut ang_jac1 = self.cmat1_basis[locked_axis];
+        #[cfg(feature = "dim2")]
+        let mut ang_jac2 = self.cmat2_basis[locked_axis];
+        #[cfg(feature = "dim3")]
+        let mut ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
+        #[cfg(feature = "dim3")]
+        let mut ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
+
+        ang_jac1 = body1.sqrt_ii * ang_jac1;
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        JointVelocityConstraint {
+            joint_id,
+            mj_lambda1: body1.mj_lambda,
+            mj_lambda2: body2.mj_lambda,
+            im1: body1.im,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            rhs: rhs_wo_bias + rhs_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn limit_angular<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        limited_axis: usize,
+        limits: [N; 2],
+        writeback_id: WritebackId,
+    ) -> JointVelocityConstraint<N, LANES> {
+        let zero = N::zero();
+        let half = N::splat(0.5);
+        let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()];
+        #[cfg(feature = "dim2")]
+        let s_ang = self.ang_err.im;
+        #[cfg(feature = "dim3")]
+        let s_ang = self.ang_err.imag()[limited_axis];
+        let min_enabled = s_ang.simd_lt(s_limits[0]);
+        let max_enabled = s_limits[1].simd_lt(s_ang);
+
+        let impulse_bounds = [
+            N::splat(-Real::INFINITY).select(min_enabled, zero),
+            N::splat(Real::INFINITY).select(max_enabled, zero),
+        ];
+
+        #[cfg(feature = "dim2")]
+        let ang_jac = self.ang_basis[limited_axis];
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
+            - (s_limits[0] - s_ang).simd_max(zero))
+            * N::splat(erp_inv_dt);
+
+        let ang_jac1 = body1.sqrt_ii * ang_jac;
+        let ang_jac2 = body2.sqrt_ii * ang_jac;
+
+        JointVelocityConstraint {
+            joint_id,
+            mj_lambda1: body1.mj_lambda,
+            mj_lambda2: body2.mj_lambda,
+            im1: body1.im,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds,
+            lin_jac: na::zero(),
+            ang_jac1,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            rhs: rhs_wo_bias + rhs_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn motor_angular<const LANES: usize>(
+        &self,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        _motor_axis: usize,
+        motor_params: &MotorParameters<N>,
+        writeback_id: WritebackId,
+    ) -> JointVelocityConstraint<N, LANES> {
+        // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_jac = N::one();
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.basis.column(_motor_axis).into_owned();
+
+        let mut rhs_wo_bias = N::zero();
+        if motor_params.stiffness != N::zero() {
+            #[cfg(feature = "dim2")]
+            let s_ang_dist = self.ang_err.im;
+            #[cfg(feature = "dim3")]
+            let s_ang_dist = self.ang_err.imag()[_motor_axis];
+            let s_target_ang = motor_params.target_pos.simd_sin();
+            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+        }
+
+        if motor_params.damping != N::zero() {
+            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+            rhs_wo_bias +=
+                (dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
+        }
+
+        let ang_jac1 = body1.sqrt_ii * ang_jac;
+        let ang_jac2 = body2.sqrt_ii * ang_jac;
+
+        JointVelocityConstraint {
+            joint_id,
+            mj_lambda1: body1.mj_lambda,
+            mj_lambda2: body2.mj_lambda,
+            im1: body1.im,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
+            lin_jac: na::zero(),
+            ang_jac1,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            rhs: rhs_wo_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn lock_angular<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        locked_axis: usize,
+        writeback_id: WritebackId,
+    ) -> JointVelocityConstraint<N, LANES> {
+        #[cfg(feature = "dim2")]
+        let ang_jac = self.ang_basis[locked_axis];
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+
+        let erp_inv_dt = params.erp_inv_dt();
+        #[cfg(feature = "dim2")]
+        let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
+        #[cfg(feature = "dim3")]
+        let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
+
+        let ang_jac1 = body1.sqrt_ii * ang_jac;
+        let ang_jac2 = body2.sqrt_ii * ang_jac;
+
+        JointVelocityConstraint {
+            joint_id,
+            mj_lambda1: body1.mj_lambda,
+            mj_lambda2: body2.mj_lambda,
+            im1: body1.im,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
+            lin_jac: na::zero(),
+            ang_jac1,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            rhs: rhs_wo_bias + rhs_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    /// Orthogonalize the constraints and set their inv_lhs field.
+    pub fn finalize_constraints<const LANES: usize>(
+        constraints: &mut [JointVelocityConstraint<N, LANES>],
+    ) {
+        let len = constraints.len();
+        let imsum = constraints[0].im1 + constraints[0].im2;
+
+        // Use the modified Gram-Schmidt orthogonalization.
+        for j in 0..len {
+            let c_j = &mut constraints[j];
+            let dot_jj = c_j.lin_jac.norm_squared() * imsum
+                + c_j.ang_jac1.gdot(c_j.ang_jac1)
+                + c_j.ang_jac2.gdot(c_j.ang_jac2);
+            let inv_dot_jj = crate::utils::simd_inv(dot_jj);
+            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+
+            if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
+                // Don't remove constraints with limited forces from the others
+                // because they may not deliver the necessary forces to fulfill
+                // the removed parts of other constraints.
+                continue;
+            }
+
+            for i in (j + 1)..len {
+                let (c_i, c_j) = constraints.index_mut_const(i, j);
+                let dot_ij = c_i.lin_jac.dot(&c_j.lin_jac) * imsum
+                    + c_i.ang_jac1.gdot(c_j.ang_jac1)
+                    + c_i.ang_jac2.gdot(c_j.ang_jac2);
+                let coeff = dot_ij * inv_dot_jj;
+
+                c_i.lin_jac -= c_j.lin_jac * coeff;
+                c_i.ang_jac1 -= c_j.ang_jac1 * coeff;
+                c_i.ang_jac2 -= c_j.ang_jac2 * coeff;
+                c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
+                c_i.rhs -= c_j.rhs * coeff;
+            }
+        }
+    }
+
+    pub fn limit_linear_ground<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        limited_axis: usize,
+        limits: [N; 2],
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        let zero = N::zero();
+        let lin_jac = self.basis.column(limited_axis).into_owned();
+        let dist = self.lin_err.dot(&lin_jac);
+        let min_enabled = dist.simd_lt(limits[0]);
+        let max_enabled = limits[1].simd_lt(dist);
+
+        let impulse_bounds = [
+            N::splat(-Real::INFINITY).select(min_enabled, zero),
+            N::splat(Real::INFINITY).select(max_enabled, zero),
+        ];
+
+        let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let mut ang_jac2 = self.cmat2_basis[limited_axis];
+        #[cfg(feature = "dim3")]
+        let mut ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned();
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero))
+            * N::splat(erp_inv_dt);
+
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: zero,
+            impulse_bounds,
+            lin_jac,
+            ang_jac2,
+            inv_lhs: zero, // Will be set during ortogonalization.
+            rhs: rhs_wo_bias + rhs_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn motor_linear_ground<const LANES: usize>(
+        &self,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        motor_axis: usize,
+        motor_params: &MotorParameters<N>,
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        let lin_jac = self.basis.column(motor_axis).into_owned();
+        let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let mut ang_jac2 = self.cmat2_basis[motor_axis];
+        #[cfg(feature = "dim3")]
+        let mut ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
+
+        let mut rhs_wo_bias = N::zero();
+        if motor_params.stiffness != N::zero() {
+            let dist = self.lin_err.dot(&lin_jac);
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+        }
+
+        if motor_params.damping != N::zero() {
+            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) * motor_params.damping;
+        }
+
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
+            lin_jac,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            rhs: rhs_wo_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn lock_linear_ground<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        locked_axis: usize,
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        let lin_jac = self.basis.column(locked_axis).into_owned();
+        let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let mut ang_jac2 = self.cmat2_basis[locked_axis];
+        #[cfg(feature = "dim3")]
+        let mut ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
+
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
+            lin_jac,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            rhs: rhs_wo_bias + rhs_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn motor_angular_ground<const LANES: usize>(
+        &self,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        _motor_axis: usize,
+        motor_params: &MotorParameters<N>,
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_jac = N::one();
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.basis.column(_motor_axis).into_owned();
+
+        let mut rhs_wo_bias = N::zero();
+        if motor_params.stiffness != N::zero() {
+            #[cfg(feature = "dim2")]
+            let s_ang_dist = self.ang_err.im;
+            #[cfg(feature = "dim3")]
+            let s_ang_dist = self.ang_err.imag()[_motor_axis];
+            let s_target_ang = motor_params.target_pos.simd_sin();
+            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+        }
+
+        if motor_params.damping != N::zero() {
+            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+            rhs_wo_bias +=
+                (dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
+        }
+
+        let ang_jac2 = body2.sqrt_ii * ang_jac;
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
+            lin_jac: na::zero(),
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            rhs: rhs_wo_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn limit_angular_ground<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        limited_axis: usize,
+        limits: [N; 2],
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        let zero = N::zero();
+        let half = N::splat(0.5);
+        let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()];
+        #[cfg(feature = "dim2")]
+        let s_ang = self.ang_err.im;
+        #[cfg(feature = "dim3")]
+        let s_ang = self.ang_err.imag()[limited_axis];
+        let min_enabled = s_ang.simd_lt(s_limits[0]);
+        let max_enabled = s_limits[1].simd_lt(s_ang);
+
+        let impulse_bounds = [
+            N::splat(-Real::INFINITY).select(min_enabled, zero),
+            N::splat(Real::INFINITY).select(max_enabled, zero),
+        ];
+
+        #[cfg(feature = "dim2")]
+        let ang_jac = self.ang_basis[limited_axis];
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+
+        let erp_inv_dt = params.erp_inv_dt();
+        let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
+            - (s_limits[0] - s_ang).simd_max(zero))
+            * N::splat(erp_inv_dt);
+
+        let ang_jac2 = body2.sqrt_ii * ang_jac;
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: zero,
+            impulse_bounds,
+            lin_jac: na::zero(),
+            ang_jac2,
+            inv_lhs: zero, // Will be set during ortogonalization.
+            rhs: rhs_wo_bias + rhs_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    pub fn lock_angular_ground<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        locked_axis: usize,
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        #[cfg(feature = "dim2")]
+        let ang_jac = self.ang_basis[locked_axis];
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+
+        let erp_inv_dt = params.erp_inv_dt();
+        #[cfg(feature = "dim2")]
+        let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
+        #[cfg(feature = "dim3")]
+        let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
+
+        let ang_jac2 = body2.sqrt_ii * ang_jac;
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
+            lin_jac: na::zero(),
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            rhs: rhs_wo_bias + rhs_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
+    /// Orthogonalize the constraints and set their inv_lhs field.
+    pub fn finalize_ground_constraints<const LANES: usize>(
+        constraints: &mut [JointVelocityGroundConstraint<N, LANES>],
+    ) {
+        let len = constraints.len();
+        let imsum = constraints[0].im2;
+
+        // Use the modified Gram-Schmidt orthogonalization.
+        for j in 0..len {
+            let c_j = &mut constraints[j];
+            let dot_jj = c_j.lin_jac.norm_squared() * imsum + c_j.ang_jac2.gdot(c_j.ang_jac2);
+            let inv_dot_jj = crate::utils::simd_inv(dot_jj);
+            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+
+            if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
+                // Don't remove constraints with limited forces from the others
+                // because they may not deliver the necessary forces to fulfill
+                // the removed parts of other constraints.
+                continue;
+            }
+
+            for i in (j + 1)..len {
+                let (c_i, c_j) = constraints.index_mut_const(i, j);
+                let dot_ij =
+                    c_i.lin_jac.dot(&c_j.lin_jac) * imsum + c_i.ang_jac2.gdot(c_j.ang_jac2);
+                let coeff = dot_ij * inv_dot_jj;
+
+                c_i.lin_jac -= c_j.lin_jac * coeff;
+                c_i.ang_jac2 -= c_j.ang_jac2 * coeff;
+                c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
+                c_i.rhs -= c_j.rhs * coeff;
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/mod.rs

@@ -1,102 +1,13 @@
-pub(self) use ball_position_constraint::{BallPositionConstraint, BallPositionGroundConstraint};
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use ball_position_constraint_wide::{
-    WBallPositionConstraint, WBallPositionGroundConstraint,
-};
-pub(self) use ball_velocity_constraint::{BallVelocityConstraint, BallVelocityGroundConstraint};
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use ball_velocity_constraint_wide::{
-    WBallVelocityConstraint, WBallVelocityGroundConstraint,
-};
-pub(self) use fixed_position_constraint::{FixedPositionConstraint, FixedPositionGroundConstraint};
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use fixed_position_constraint_wide::{
-    WFixedPositionConstraint, WFixedPositionGroundConstraint,
-};
-pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint};
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use fixed_velocity_constraint_wide::{
-    WFixedVelocityConstraint, WFixedVelocityGroundConstraint,
-};
-// pub(self) use generic_position_constraint::{
-//     GenericPositionConstraint, GenericPositionGroundConstraint,
-// };
-// #[cfg(feature = "simd-is-enabled")]
-// pub(self) use generic_position_constraint_wide::{
-//     WGenericPositionConstraint, WGenericPositionGroundConstraint,
-// };
-// pub(self) use generic_velocity_constraint::{
-//     GenericVelocityConstraint, GenericVelocityGroundConstraint,
-// };
-// #[cfg(feature = "simd-is-enabled")]
-// pub(self) use generic_velocity_constraint_wide::{
-//     WGenericVelocityConstraint, WGenericVelocityGroundConstraint,
-// };
+pub use joint_velocity_constraint::{MotorParameters, SolverBody, WritebackId};
 
-pub(crate) use joint_constraint::AnyJointVelocityConstraint;
-pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
-pub(self) use prismatic_position_constraint::{
-    PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
-};
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use prismatic_position_constraint_wide::{
-    WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
-};
-pub(self) use prismatic_velocity_constraint::{
-    PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
-};
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use prismatic_velocity_constraint_wide::{
-    WPrismaticVelocityConstraint, WPrismaticVelocityGroundConstraint,
-};
-#[cfg(feature = "dim3")]
-pub(self) use revolute_position_constraint::{
-    RevolutePositionConstraint, RevolutePositionGroundConstraint,
-};
-#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
-pub(self) use revolute_position_constraint_wide::{
-    WRevolutePositionConstraint, WRevolutePositionGroundConstraint,
-};
-#[cfg(feature = "dim3")]
-pub(self) use revolute_velocity_constraint::{
-    RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint,
-};
-#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
-pub(self) use revolute_velocity_constraint_wide::{
-    WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint,
+pub use joint_constraint::AnyJointVelocityConstraint;
+pub use joint_generic_velocity_constraint::{
+    JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
 };
+pub use joint_velocity_constraint_builder::JointVelocityConstraintBuilder;
 
-mod ball_position_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod ball_position_constraint_wide;
-mod ball_velocity_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod ball_velocity_constraint_wide;
-mod fixed_position_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod fixed_position_constraint_wide;
-mod fixed_velocity_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod fixed_velocity_constraint_wide;
-// mod generic_position_constraint;
-// #[cfg(feature = "simd-is-enabled")]
-// mod generic_position_constraint_wide;
-// mod generic_velocity_constraint;
-// #[cfg(feature = "simd-is-enabled")]
-// mod generic_velocity_constraint_wide;
 mod joint_constraint;
-mod joint_position_constraint;
-mod prismatic_position_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod prismatic_position_constraint_wide;
-mod prismatic_velocity_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod prismatic_velocity_constraint_wide;
-#[cfg(feature = "dim3")]
-mod revolute_position_constraint;
-#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
-mod revolute_position_constraint_wide;
-#[cfg(feature = "dim3")]
-mod revolute_velocity_constraint;
-#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
-mod revolute_velocity_constraint_wide;
+mod joint_generic_velocity_constraint;
+mod joint_generic_velocity_constraint_builder;
+mod joint_velocity_constraint;
+mod joint_velocity_constraint_builder;

src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs

@@ -1,182 +0,0 @@
-use crate::dynamics::{
-    IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-};
-use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
-use crate::utils::WAngularInertia;
-use na::Unit;
-
-#[derive(Debug)]
-pub(crate) struct PrismaticPositionConstraint {
-    position1: usize,
-    position2: usize,
-
-    im1: Real,
-    im2: Real,
-
-    ii1: AngularInertia<Real>,
-    ii2: AngularInertia<Real>,
-
-    lin_inv_lhs: Real,
-    ang_inv_lhs: AngularInertia<Real>,
-
-    limits: [Real; 2],
-
-    local_frame1: Isometry<Real>,
-    local_frame2: Isometry<Real>,
-
-    local_axis1: Unit<Vector<Real>>,
-    local_axis2: Unit<Vector<Real>>,
-}
-
-impl PrismaticPositionConstraint {
-    pub fn from_params(
-        rb1: (&RigidBodyMassProps, &RigidBodyIds),
-        rb2: (&RigidBodyMassProps, &RigidBodyIds),
-        cparams: &PrismaticJoint,
-    ) -> Self {
-        let (mprops1, ids1) = rb1;
-        let (mprops2, ids2) = rb2;
-
-        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let im1 = mprops1.effective_inv_mass;
-        let im2 = mprops2.effective_inv_mass;
-        let lin_inv_lhs = 1.0 / (im1 + im2);
-        let ang_inv_lhs = (ii1 + ii2).inverse();
-
-        Self {
-            im1,
-            im2,
-            ii1,
-            ii2,
-            lin_inv_lhs,
-            ang_inv_lhs,
-            local_frame1: cparams.local_frame1(),
-            local_frame2: cparams.local_frame2(),
-            local_axis1: cparams.local_axis1,
-            local_axis2: cparams.local_axis2,
-            position1: ids1.active_set_offset,
-            position2: ids2.active_set_offset,
-            limits: cparams.limits,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position1 = positions[self.position1 as usize];
-        let mut position2 = positions[self.position2 as usize];
-
-        // Angular correction.
-        let frame1 = position1 * self.local_frame1;
-        let frame2 = position2 * self.local_frame2;
-        let ang_err = frame2.rotation * frame1.rotation.inverse();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self
-            .ang_inv_lhs
-            .transform_vector(ang_err.angle() * params.joint_erp);
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self
-            .ang_inv_lhs
-            .transform_vector(ang_err.scaled_axis() * params.joint_erp);
-        position1.rotation =
-            Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
-        position2.rotation =
-            Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
-
-        // Linear correction.
-        let anchor1 = position1 * Point::from(self.local_frame1.translation.vector);
-        let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
-        let axis1 = position1 * self.local_axis1;
-        let dpos = anchor2 - anchor1;
-        let limit_err = dpos.dot(&axis1);
-        let mut err = dpos - *axis1 * limit_err;
-
-        if limit_err < self.limits[0] {
-            err += *axis1 * (limit_err - self.limits[0]);
-        } else if limit_err > self.limits[1] {
-            err += *axis1 * (limit_err - self.limits[1]);
-        }
-
-        let impulse = err * (self.lin_inv_lhs * params.joint_erp);
-        position1.translation.vector += self.im1 * impulse;
-        position2.translation.vector -= self.im2 * impulse;
-
-        positions[self.position1 as usize] = position1;
-        positions[self.position2 as usize] = position2;
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct PrismaticPositionGroundConstraint {
-    position2: usize,
-    frame1: Isometry<Real>,
-    local_frame2: Isometry<Real>,
-    axis1: Unit<Vector<Real>>,
-    local_axis2: Unit<Vector<Real>>,
-    limits: [Real; 2],
-}
-
-impl PrismaticPositionGroundConstraint {
-    pub fn from_params(
-        rb1: &RigidBodyPosition,
-        rb2: (&RigidBodyMassProps, &RigidBodyIds),
-        cparams: &PrismaticJoint,
-        flipped: bool,
-    ) -> Self {
-        let poss1 = rb1;
-        let (_, ids2) = rb2;
-
-        let frame1;
-        let local_frame2;
-        let axis1;
-        let local_axis2;
-
-        if flipped {
-            frame1 = poss1.next_position * cparams.local_frame2();
-            local_frame2 = cparams.local_frame1();
-            axis1 = poss1.next_position * cparams.local_axis2;
-            local_axis2 = cparams.local_axis1;
-        } else {
-            frame1 = poss1.next_position * cparams.local_frame1();
-            local_frame2 = cparams.local_frame2();
-            axis1 = poss1.next_position * cparams.local_axis1;
-            local_axis2 = cparams.local_axis2;
-        };
-
-        Self {
-            frame1,
-            local_frame2,
-            axis1,
-            local_axis2,
-            position2: ids2.active_set_offset,
-            limits: cparams.limits,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position2 = positions[self.position2 as usize];
-
-        // Angular correction.
-        let frame2 = position2 * self.local_frame2;
-        let ang_err = frame2.rotation * self.frame1.rotation.inverse();
-        position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
-
-        // Linear correction.
-        let anchor1 = Point::from(self.frame1.translation.vector);
-        let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
-        let dpos = anchor2 - anchor1;
-        let limit_err = dpos.dot(&self.axis1);
-        let mut err = dpos - *self.axis1 * limit_err;
-
-        if limit_err < self.limits[0] {
-            err += *self.axis1 * (limit_err - self.limits[0]);
-        } else if limit_err > self.limits[1] {
-            err += *self.axis1 * (limit_err - self.limits[1]);
-        }
-
-        // NOTE: no need to divide by im2 just to multiply right after.
-        let impulse = err * params.joint_erp;
-        position2.translation.vector -= impulse;
-
-        positions[self.position2 as usize] = position2;
-    }
-}

src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs

@@ -1,71 +0,0 @@
-use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint};
-use crate::dynamics::{
-    IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-};
-use crate::math::{Isometry, Real, SIMD_WIDTH};
-
-// TODO: this does not uses SIMD optimizations yet.
-#[derive(Debug)]
-pub(crate) struct WPrismaticPositionConstraint {
-    constraints: [PrismaticPositionConstraint; SIMD_WIDTH],
-}
-
-impl WPrismaticPositionConstraint {
-    pub fn from_params(
-        rbs1: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&PrismaticJoint; SIMD_WIDTH],
-    ) -> Self {
-        Self {
-            constraints: gather![|ii| PrismaticPositionConstraint::from_params(
-                (rbs1.0[ii], rbs1.1[ii]),
-                (rbs2.0[ii], rbs2.1[ii]),
-                cparams[ii]
-            )],
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        for constraint in &self.constraints {
-            constraint.solve(params, positions);
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WPrismaticPositionGroundConstraint {
-    constraints: [PrismaticPositionGroundConstraint; SIMD_WIDTH],
-}
-
-impl WPrismaticPositionGroundConstraint {
-    pub fn from_params(
-        rbs1: [&RigidBodyPosition; SIMD_WIDTH],
-        rbs2: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&PrismaticJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        Self {
-            constraints: gather![|ii| PrismaticPositionGroundConstraint::from_params(
-                rbs1[ii],
-                (rbs2.0[ii], rbs2.1[ii]),
-                cparams[ii],
-                flipped[ii]
-            )],
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        for constraint in &self.constraints {
-            constraint.solve(params, positions);
-        }
-    }
-}

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

@@ -1,859 +0,0 @@
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-};
-use crate::math::{AngularInertia, Real, Vector};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
-#[cfg(feature = "dim3")]
-use na::{Cholesky, Matrix3x2, Matrix5, Vector5};
-#[cfg(feature = "dim2")]
-use {
-    na::{Matrix2, Vector2},
-    parry::utils::SdpMatrix2,
-};
-
-#[cfg(feature = "dim2")]
-const LIN_IMPULSE_DIM: usize = 1;
-#[cfg(feature = "dim3")]
-const LIN_IMPULSE_DIM: usize = 2;
-
-#[derive(Debug)]
-pub(crate) struct PrismaticVelocityConstraint {
-    mj_lambda1: usize,
-    mj_lambda2: usize,
-
-    joint_id: JointIndex,
-
-    r1: Vector<Real>,
-    r2: Vector<Real>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix5<Real>,
-    #[cfg(feature = "dim3")]
-    rhs: Vector5<Real>,
-    #[cfg(feature = "dim3")]
-    impulse: Vector5<Real>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix2<Real>,
-    #[cfg(feature = "dim2")]
-    rhs: Vector2<Real>,
-    #[cfg(feature = "dim2")]
-    impulse: Vector2<Real>,
-
-    motor_axis1: Vector<Real>,
-    motor_axis2: Vector<Real>,
-    motor_impulse: Real,
-    motor_rhs: Real,
-    motor_inv_lhs: Real,
-    motor_max_impulse: Real,
-
-    limits_active: bool,
-    limits_impulse: Real,
-    /// World-coordinate direction of the limit force on rb2.
-    /// The force direction on rb1 is opposite (Newton's third law)..
-    limits_forcedir2: Vector<Real>,
-    limits_rhs: Real,
-    limits_inv_lhs: Real,
-    /// min/max applied impulse due to limits
-    limits_impulse_limits: (Real, Real),
-
-    #[cfg(feature = "dim2")]
-    basis1: Vector2<Real>,
-    #[cfg(feature = "dim3")]
-    basis1: Matrix3x2<Real>,
-
-    im1: Real,
-    im2: Real,
-
-    ii1_sqrt: AngularInertia<Real>,
-    ii2_sqrt: AngularInertia<Real>,
-}
-
-impl PrismaticVelocityConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        rb2: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        joint: &PrismaticJoint,
-    ) -> Self {
-        let (poss1, vels1, mprops1, ids1) = rb1;
-        let (poss2, vels2, mprops2, ids2) = rb2;
-
-        // Linear part.
-        let anchor1 = poss1.position * joint.local_anchor1;
-        let anchor2 = poss2.position * joint.local_anchor2;
-        let axis1 = poss1.position * joint.local_axis1;
-        let axis2 = poss2.position * joint.local_axis2;
-
-        #[cfg(feature = "dim2")]
-        let basis1 = poss1.position * joint.basis1[0];
-        #[cfg(feature = "dim3")]
-        let basis1 = Matrix3x2::from_columns(&[
-            poss1.position * joint.basis1[0],
-            poss1.position * joint.basis1[1],
-        ]);
-
-        let im1 = mprops1.effective_inv_mass;
-        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1 - mprops1.world_com;
-        let r1_mat = r1.gcross_matrix();
-
-        let im2 = mprops2.effective_inv_mass;
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let r2 = anchor2 - mprops2.world_com;
-        let r2_mat = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D.
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let r1_mat_b1 = r1_mat * basis1;
-            let r2_mat_b1 = r2_mat * basis1;
-
-            lhs = Matrix5::zeros();
-            let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1)
-                + ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
-            let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
-            let lhs11 = (ii1 + ii2).into_matrix();
-            lhs.fixed_slice_mut::<2, 2>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
-        }
-
-        #[cfg(feature = "dim2")]
-        {
-            let b1r1 = basis1.dot(&r1_mat);
-            let b2r2 = basis1.dot(&r2_mat);
-            let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2;
-            let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2;
-            let m22 = ii1 + ii2;
-            lhs = SdpMatrix2::new(m11, m12, m22);
-        }
-
-        let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
-        let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
-
-        // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
-        // for which a textbook inverse is still efficient.
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.inverse_unchecked().into_matrix();
-        #[cfg(feature = "dim3")]
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let angvel_err = vels2.angvel - vels1.angvel;
-
-        #[cfg(feature = "dim2")]
-        let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
-        #[cfg(feature = "dim3")]
-        let mut rhs = Vector5::new(
-            linvel_err.x,
-            linvel_err.y,
-            angvel_err.x,
-            angvel_err.y,
-            angvel_err.z,
-        ) * params.velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
-
-            let frame1 = poss1.position * joint.local_frame1();
-            let frame2 = poss2.position * joint.local_frame2();
-            let ang_err = frame2.rotation * frame1.rotation.inverse();
-
-            #[cfg(feature = "dim2")]
-            {
-                rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
-            }
-            #[cfg(feature = "dim3")]
-            {
-                let ang_err = ang_err.scaled_axis();
-                rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
-                    * velocity_based_erp_inv_dt;
-            }
-        }
-
-        /*
-         * Setup motor.
-         */
-        let mut motor_rhs = 0.0;
-        let mut motor_inv_lhs = 0.0;
-        let gcross1 = r1.gcross(*axis1);
-        let gcross2 = r2.gcross(*axis2);
-
-        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
-            params.dt,
-            joint.motor_stiffness,
-            joint.motor_damping,
-        );
-
-        if stiffness != 0.0 {
-            let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1);
-            motor_rhs += (dist - joint.motor_target_pos) * stiffness;
-        }
-
-        if damping != 0.0 {
-            let curr_vel = vels2.linvel.dot(&axis2) + vels2.angvel.gdot(gcross2)
-                - vels1.linvel.dot(&axis1)
-                - vels1.angvel.gdot(gcross1);
-            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
-        }
-
-        if stiffness != 0.0 || damping != 0.0 {
-            motor_inv_lhs = if keep_lhs {
-                let inv_projected_mass = crate::utils::inv(
-                    im1 + im2
-                        + gcross1.gdot(ii1.transform_vector(gcross1))
-                        + gcross2.gdot(ii2.transform_vector(gcross2)),
-                );
-
-                gamma * inv_projected_mass
-            } else {
-                gamma
-            };
-            motor_rhs /= gamma;
-        }
-
-        let motor_impulse = na::clamp(
-            joint.motor_impulse,
-            -joint.motor_max_impulse,
-            joint.motor_max_impulse,
-        );
-
-        // Setup limit constraint.
-        let mut limits_active = false;
-        let limits_forcedir2 = axis2.into_inner(); // hopefully axis1 is colinear with axis2
-        let mut limits_rhs = 0.0;
-        let mut limits_impulse = 0.0;
-        let mut limits_inv_lhs = 0.0;
-        let mut limits_impulse_limits = (0.0, 0.0);
-
-        if joint.limits_enabled {
-            let danchor = anchor2 - anchor1;
-            let dist = danchor.dot(&axis1);
-
-            // TODO: we should allow predictive constraint activation.
-
-            let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
-            let min_enabled = dist < min_limit;
-            let max_enabled = max_limit < dist;
-
-            limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
-            limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
-            limits_active = min_enabled || max_enabled;
-
-            if limits_active {
-                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
-                    * params.velocity_solve_fraction;
-
-                limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
-                    * velocity_based_erp_inv_dt;
-
-                limits_inv_lhs = crate::utils::inv(
-                    im1 + im2
-                        + gcross1.gdot(ii1.transform_vector(gcross1))
-                        + gcross2.gdot(ii2.transform_vector(gcross2)),
-                );
-
-                limits_impulse = joint
-                    .limits_impulse
-                    .max(limits_impulse_limits.0)
-                    .min(limits_impulse_limits.1);
-            }
-        }
-
-        PrismaticVelocityConstraint {
-            joint_id,
-            mj_lambda1: ids1.active_set_offset,
-            mj_lambda2: ids2.active_set_offset,
-            im1,
-            ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
-            im2,
-            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
-            impulse: joint.impulse * params.warmstart_coeff,
-            limits_active,
-            limits_impulse: limits_impulse * params.warmstart_coeff,
-            limits_forcedir2,
-            limits_rhs,
-            limits_inv_lhs,
-            limits_impulse_limits,
-            motor_rhs,
-            motor_inv_lhs,
-            motor_impulse,
-            motor_axis1: *axis1,
-            motor_axis2: *axis2,
-            motor_max_impulse: joint.motor_max_impulse,
-            basis1,
-            inv_lhs,
-            rhs,
-            r1,
-            r2,
-        }
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse.y;
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<3>(2).into_owned();
-
-        mj_lambda1.linear += self.im1 * lin_impulse;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        // Warmstart motors.
-        if self.motor_impulse != 0.0 {
-            let lin_impulse1 = self.motor_axis1 * self.motor_impulse;
-            let lin_impulse2 = self.motor_axis2 * self.motor_impulse;
-
-            mj_lambda1.linear += lin_impulse1 * self.im1;
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
-
-            mj_lambda2.linear -= lin_impulse2 * self.im2;
-            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
-        }
-
-        // Warmstart limits.
-        if self.limits_active {
-            let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse;
-            let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse;
-            mj_lambda1.linear += self.im1 * limit_impulse1;
-            mj_lambda1.angular += self
-                .ii1_sqrt
-                .transform_vector(self.r1.gcross(limit_impulse1));
-            mj_lambda2.linear += self.im2 * limit_impulse2;
-            mj_lambda2.angular += self
-                .ii2_sqrt
-                .transform_vector(self.r2.gcross(limit_impulse2));
-        }
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
-        let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1));
-        let ang_dvel = ang_vel2 - ang_vel1;
-        #[cfg(feature = "dim2")]
-        let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse.y;
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<3>(2).into_owned();
-
-        mj_lambda1.linear += self.im1 * lin_impulse;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-    }
-
-    fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.limits_active {
-            let limits_forcedir1 = -self.limits_forcedir2;
-            let limits_forcedir2 = self.limits_forcedir2;
-
-            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
-                + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
-                + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs)
-                .max(self.limits_impulse_limits.0)
-                .min(self.limits_impulse_limits.1);
-            let dimpulse = new_impulse - self.limits_impulse;
-            self.limits_impulse = new_impulse;
-
-            let lin_impulse1 = limits_forcedir1 * dimpulse;
-            let lin_impulse2 = limits_forcedir2 * dimpulse;
-
-            mj_lambda1.linear += self.im1 * lin_impulse1;
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
-            mj_lambda2.linear += self.im2 * lin_impulse2;
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
-        }
-    }
-
-    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.motor_inv_lhs != 0.0 {
-            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let dvel = self
-                .motor_axis2
-                .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
-                - self
-                    .motor_axis1
-                    .dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
-                + self.motor_rhs;
-            let new_impulse = na::clamp(
-                self.motor_impulse + dvel * self.motor_inv_lhs,
-                -self.motor_max_impulse,
-                self.motor_max_impulse,
-            );
-            let dimpulse = new_impulse - self.motor_impulse;
-            self.motor_impulse = new_impulse;
-
-            let lin_impulse1 = self.motor_axis1 * dimpulse;
-            let lin_impulse2 = self.motor_axis2 * dimpulse;
-
-            mj_lambda1.linear += lin_impulse1 * self.im1;
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
-            mj_lambda2.linear -= lin_impulse2 * self.im2;
-            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
-        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
-        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
-            revolute.impulse = self.impulse;
-            revolute.motor_impulse = self.motor_impulse;
-            revolute.limits_impulse = self.limits_impulse;
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct PrismaticVelocityGroundConstraint {
-    mj_lambda2: usize,
-
-    joint_id: JointIndex,
-
-    r2: Vector<Real>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix2<Real>,
-    #[cfg(feature = "dim2")]
-    rhs: Vector2<Real>,
-    #[cfg(feature = "dim2")]
-    impulse: Vector2<Real>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix5<Real>,
-    #[cfg(feature = "dim3")]
-    rhs: Vector5<Real>,
-    #[cfg(feature = "dim3")]
-    impulse: Vector5<Real>,
-
-    limits_active: bool,
-    limits_forcedir2: Vector<Real>,
-    limits_impulse: Real,
-    limits_rhs: Real,
-    /// min/max applied impulse due to limits
-    limits_impulse_limits: (Real, Real),
-
-    axis2: Vector<Real>,
-    motor_impulse: Real,
-    motor_rhs: Real,
-    motor_inv_lhs: Real,
-    motor_max_impulse: Real,
-
-    #[cfg(feature = "dim2")]
-    basis1: Vector2<Real>,
-    #[cfg(feature = "dim3")]
-    basis1: Matrix3x2<Real>,
-
-    im2: Real,
-    ii2_sqrt: AngularInertia<Real>,
-}
-
-impl PrismaticVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
-        rb2: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        joint: &PrismaticJoint,
-        flipped: bool,
-    ) -> Self {
-        let (poss1, vels1, mprops1) = rb1;
-        let (poss2, vels2, mprops2, ids2) = rb2;
-
-        let anchor2;
-        let anchor1;
-        let axis2;
-        let axis1;
-        let basis1;
-
-        if flipped {
-            anchor2 = poss2.position * joint.local_anchor1;
-            anchor1 = poss1.position * joint.local_anchor2;
-            axis2 = poss2.position * joint.local_axis1;
-            axis1 = poss1.position * joint.local_axis2;
-            #[cfg(feature = "dim2")]
-            {
-                basis1 = poss1.position * joint.basis2[0];
-            }
-            #[cfg(feature = "dim3")]
-            {
-                basis1 = Matrix3x2::from_columns(&[
-                    poss1.position * joint.basis2[0],
-                    poss1.position * joint.basis2[1],
-                ]);
-            }
-        } else {
-            anchor2 = poss2.position * joint.local_anchor2;
-            anchor1 = poss1.position * joint.local_anchor1;
-            axis2 = poss2.position * joint.local_axis2;
-            axis1 = poss1.position * joint.local_axis1;
-            #[cfg(feature = "dim2")]
-            {
-                basis1 = poss1.position * joint.basis1[0];
-            }
-            #[cfg(feature = "dim3")]
-            {
-                basis1 = Matrix3x2::from_columns(&[
-                    poss1.position * joint.basis1[0],
-                    poss1.position * joint.basis1[1],
-                ]);
-            }
-        };
-
-        // #[cfg(feature = "dim2")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // #[cfg(feature = "dim3")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .unwrap_or_else(Rotation::identity)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // let basis2 = r21 * basis1;
-        // NOTE: we use basis2 := basis1 for now is that allows
-        // simplifications of the computation without introducing
-        // much instabilities.
-
-        let im2 = mprops2.effective_inv_mass;
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1 - mprops1.world_com;
-        let r2 = anchor2 - mprops2.world_com;
-        let r2_mat = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D.
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let r2_mat_b1 = r2_mat * basis1;
-
-            lhs = Matrix5::zeros();
-            let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
-            let lhs10 = ii2 * r2_mat_b1;
-            let lhs11 = ii2.into_matrix();
-            lhs.fixed_slice_mut::<2, 2>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
-        }
-
-        #[cfg(feature = "dim2")]
-        {
-            let b2r2 = basis1.dot(&r2_mat);
-            let m11 = im2 + b2r2 * ii2 * b2r2;
-            let m12 = basis1.dot(&r2_mat) * ii2;
-            let m22 = ii2;
-            lhs = SdpMatrix2::new(m11, m12, m22);
-        }
-
-        let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
-        let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
-
-        // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
-        // for which a textbook inverse is still efficient.
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.inverse_unchecked().into_matrix();
-        #[cfg(feature = "dim3")]
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let angvel_err = vels2.angvel - vels1.angvel;
-
-        #[cfg(feature = "dim2")]
-        let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
-        #[cfg(feature = "dim3")]
-        let mut rhs = Vector5::new(
-            linvel_err.x,
-            linvel_err.y,
-            angvel_err.x,
-            angvel_err.y,
-            angvel_err.z,
-        ) * params.velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
-
-            let (frame1, frame2);
-            if flipped {
-                frame1 = poss1.position * joint.local_frame2();
-                frame2 = poss2.position * joint.local_frame1();
-            } else {
-                frame1 = poss1.position * joint.local_frame1();
-                frame2 = poss2.position * joint.local_frame2();
-            }
-
-            let ang_err = frame2.rotation * frame1.rotation.inverse();
-            #[cfg(feature = "dim2")]
-            {
-                rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
-            }
-            #[cfg(feature = "dim3")]
-            {
-                let ang_err = ang_err.scaled_axis();
-                rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
-                    * velocity_based_erp_inv_dt;
-            }
-        }
-
-        /*
-         * Setup motor.
-         */
-        let mut motor_rhs = 0.0;
-        let mut motor_inv_lhs = 0.0;
-
-        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
-            params.dt,
-            joint.motor_stiffness,
-            joint.motor_damping,
-        );
-
-        if stiffness != 0.0 {
-            let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1);
-            motor_rhs += (dist - joint.motor_target_pos) * stiffness;
-        }
-
-        if damping != 0.0 {
-            let curr_vel = vels2.linvel.dot(&axis2) - vels1.linvel.dot(&axis1);
-            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
-        }
-
-        if stiffness != 0.0 || damping != 0.0 {
-            motor_inv_lhs = if keep_lhs { gamma / im2 } else { gamma };
-            motor_rhs /= gamma;
-        }
-
-        let motor_impulse = na::clamp(
-            joint.motor_impulse,
-            -joint.motor_max_impulse,
-            joint.motor_max_impulse,
-        );
-
-        /*
-         * Setup limit constraint.
-         */
-        let mut limits_active = false;
-        let limits_forcedir2 = axis2.into_inner();
-        let mut limits_rhs = 0.0;
-        let mut limits_impulse = 0.0;
-        let mut limits_impulse_limits = (0.0, 0.0);
-
-        if joint.limits_enabled {
-            let danchor = anchor2 - anchor1;
-            let dist = danchor.dot(&axis1);
-
-            // TODO: we should allow predictive constraint activation.
-
-            let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
-            let min_enabled = dist < min_limit;
-            let max_enabled = max_limit < dist;
-
-            limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
-            limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
-
-            limits_active = min_enabled || max_enabled;
-            if limits_active {
-                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
-                    * params.velocity_solve_fraction;
-
-                limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
-                    * velocity_based_erp_inv_dt;
-
-                limits_impulse = joint
-                    .limits_impulse
-                    .max(limits_impulse_limits.0)
-                    .min(limits_impulse_limits.1);
-            }
-        }
-
-        PrismaticVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2: ids2.active_set_offset,
-            im2,
-            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
-            impulse: joint.impulse * params.warmstart_coeff,
-            limits_active,
-            limits_forcedir2,
-            limits_impulse: limits_impulse * params.warmstart_coeff,
-            limits_rhs,
-            limits_impulse_limits,
-            motor_rhs,
-            motor_inv_lhs,
-            motor_impulse,
-            motor_max_impulse: joint.motor_max_impulse,
-            basis1,
-            inv_lhs,
-            rhs,
-            r2,
-            axis2: axis2.into_inner(),
-        }
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse.y;
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<3>(2).into_owned();
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        // Warmstart motors.
-        mj_lambda2.linear -= self.axis2 * (self.im2 * self.motor_impulse);
-
-        // Warmstart limits.
-        mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * self.limits_impulse);
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let lin_dvel = self.basis1.tr_mul(&lin_vel2);
-        let ang_dvel = ang_vel2;
-        #[cfg(feature = "dim2")]
-        let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse.y;
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<3>(2).into_owned();
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-    }
-
-    fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.limits_active {
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let lin_dvel = self
-                .limits_forcedir2
-                .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
-                + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - lin_dvel / self.im2)
-                .max(self.limits_impulse_limits.0)
-                .min(self.limits_impulse_limits.1);
-            let dimpulse = new_impulse - self.limits_impulse;
-            self.limits_impulse = new_impulse;
-
-            mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse);
-        }
-    }
-
-    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.motor_inv_lhs != 0.0 {
-            let lin_dvel = self.axis2.dot(&mj_lambda2.linear) + self.motor_rhs;
-            let new_impulse = na::clamp(
-                self.motor_impulse + lin_dvel * self.motor_inv_lhs,
-                -self.motor_max_impulse,
-                self.motor_max_impulse,
-            );
-            let dimpulse = new_impulse - self.motor_impulse;
-            self.motor_impulse = new_impulse;
-
-            mj_lambda2.linear -= self.axis2 * (self.im2 * dimpulse);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        self.solve_limits(&mut mj_lambda2);
-        self.solve_motors(&mut mj_lambda2);
-        self.solve_dofs(&mut mj_lambda2);
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    // TODO: duplicated code with the non-ground constraint.
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
-            revolute.impulse = self.impulse;
-            revolute.motor_impulse = self.motor_impulse;
-            revolute.limits_impulse = self.limits_impulse;
-        }
-    }
-}

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs

@@ -1,848 +0,0 @@
-use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
-
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-};
-use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
-};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
-
-#[cfg(feature = "dim3")]
-use na::{Cholesky, Matrix3x2, Matrix5, Vector3, Vector5};
-
-#[cfg(feature = "dim2")]
-use {
-    na::{Matrix2, Vector2},
-    parry::utils::SdpMatrix2,
-};
-
-#[cfg(feature = "dim2")]
-const LIN_IMPULSE_DIM: usize = 1;
-
-#[cfg(feature = "dim3")]
-const LIN_IMPULSE_DIM: usize = 2;
-
-#[derive(Debug)]
-pub(crate) struct WPrismaticVelocityConstraint {
-    mj_lambda1: [usize; SIMD_WIDTH],
-    mj_lambda2: [usize; SIMD_WIDTH],
-
-    joint_id: [JointIndex; SIMD_WIDTH],
-
-    r1: Vector<SimdReal>,
-    r2: Vector<SimdReal>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix5<SimdReal>,
-    #[cfg(feature = "dim3")]
-    rhs: Vector5<SimdReal>,
-    #[cfg(feature = "dim3")]
-    impulse: Vector5<SimdReal>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix2<SimdReal>,
-    #[cfg(feature = "dim2")]
-    rhs: Vector2<SimdReal>,
-    #[cfg(feature = "dim2")]
-    impulse: Vector2<SimdReal>,
-
-    limits_active: bool,
-    limits_impulse: SimdReal,
-    limits_forcedir2: Vector<SimdReal>,
-    limits_rhs: SimdReal,
-    limits_inv_lhs: SimdReal,
-    limits_impulse_limits: (SimdReal, SimdReal),
-
-    #[cfg(feature = "dim2")]
-    basis1: Vector2<SimdReal>,
-    #[cfg(feature = "dim3")]
-    basis1: Matrix3x2<SimdReal>,
-
-    im1: SimdReal,
-    im2: SimdReal,
-
-    ii1_sqrt: AngularInertia<SimdReal>,
-    ii2_sqrt: AngularInertia<SimdReal>,
-}
-
-impl WPrismaticVelocityConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&PrismaticJoint; SIMD_WIDTH],
-    ) -> Self {
-        let (poss1, vels1, mprops1, ids1) = rbs1;
-        let (poss2, vels2, mprops2, ids2) = rbs2;
-
-        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
-        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
-        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
-        let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops1[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
-
-        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
-        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
-
-        let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
-        let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
-        let local_axis1 = Vector::from(gather![|ii| *cparams[ii].local_axis1]);
-        let local_axis2 = Vector::from(gather![|ii| *cparams[ii].local_axis2]);
-
-        #[cfg(feature = "dim2")]
-        let local_basis1 = [Vector::from(gather![|ii| cparams[ii].basis1[0]])];
-        #[cfg(feature = "dim3")]
-        let local_basis1 = [
-            Vector::from(gather![|ii| cparams[ii].basis1[0]]),
-            Vector::from(gather![|ii| cparams[ii].basis1[1]]),
-        ];
-
-        #[cfg(feature = "dim2")]
-        let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
-        #[cfg(feature = "dim3")]
-        let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
-
-        let anchor1 = position1 * local_anchor1;
-        let anchor2 = position2 * local_anchor2;
-        let axis1 = position1 * local_axis1;
-        let axis2 = position2 * local_axis2;
-
-        #[cfg(feature = "dim2")]
-        let basis1 = position1 * local_basis1[0];
-        #[cfg(feature = "dim3")]
-        let basis1 =
-            Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
-
-        // #[cfg(feature = "dim2")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // #[cfg(feature = "dim3")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .unwrap_or_else(Rotation::identity)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // let basis2 = r21 * basis1;
-        // NOTE: we use basis2 := basis1 for now is that allows
-        // simplifications of the computation without introducing
-        // much instabilities.
-
-        let ii1 = ii1_sqrt.squared();
-        let r1 = anchor1 - world_com1;
-        let r1_mat = r1.gcross_matrix();
-
-        let ii2 = ii2_sqrt.squared();
-        let r2 = anchor2 - world_com2;
-        let r2_mat = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D.
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let r1_mat_b1 = r1_mat * basis1;
-            let r2_mat_b1 = r2_mat * basis1;
-
-            lhs = Matrix5::zeros();
-            let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1)
-                + ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
-            let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
-            let lhs11 = (ii1 + ii2).into_matrix();
-            lhs.fixed_slice_mut::<2, 2>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
-        }
-
-        #[cfg(feature = "dim2")]
-        {
-            let b1r1 = basis1.dot(&r1_mat);
-            let b2r2 = basis1.dot(&r2_mat);
-            let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2;
-            let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2;
-            let m22 = ii1 + ii2;
-            lhs = SdpMatrix2::new(m11, m12, m22);
-        }
-
-        let anchor_linvel1 = linvel1 + angvel1.gcross(r1);
-        let anchor_linvel2 = linvel2 + angvel2.gcross(r2);
-
-        // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
-        // for which a textbook inverse is still efficient.
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.inverse_unchecked().into_matrix();
-        #[cfg(feature = "dim3")]
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let angvel_err = angvel2 - angvel1;
-
-        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
-
-        #[cfg(feature = "dim2")]
-        let mut rhs = Vector2::new(linvel_err.x, angvel_err) * velocity_solve_fraction;
-        #[cfg(feature = "dim3")]
-        let mut rhs = Vector5::new(
-            linvel_err.x,
-            linvel_err.y,
-            angvel_err.x,
-            angvel_err.y,
-            angvel_err.z,
-        ) * velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
-
-            let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
-
-            let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1()]);
-            let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2()]);
-
-            let frame1 = position1 * local_frame1;
-            let frame2 = position2 * local_frame2;
-            let ang_err = frame2.rotation * frame1.rotation.inverse();
-
-            #[cfg(feature = "dim2")]
-            {
-                rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
-            }
-
-            #[cfg(feature = "dim3")]
-            {
-                let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
-                rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
-                    * velocity_based_erp_inv_dt;
-            }
-        }
-
-        // Setup limit constraint.
-        let zero: SimdReal = na::zero();
-        let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2
-        let mut limits_active = false;
-        let mut limits_rhs = zero;
-        let mut limits_impulse = zero;
-        let mut limits_inv_lhs = zero;
-        let mut limits_impulse_limits = (zero, zero);
-
-        let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
-        if limits_enabled.any() {
-            let danchor = anchor2 - anchor1;
-            let dist = danchor.dot(&axis1);
-
-            // TODO: we should allow predictive constraint activation.
-
-            let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
-            let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
-
-            let min_enabled = dist.simd_lt(min_limit);
-            let max_enabled = dist.simd_gt(max_limit);
-
-            limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero);
-            limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
-
-            limits_active = (min_enabled | max_enabled).any();
-            if limits_active {
-                let gcross1 = r1.gcross(axis1);
-                let gcross2 = r2.gcross(axis2);
-
-                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
-                    * velocity_solve_fraction;
-
-                limits_rhs += ((dist - max_limit).simd_max(zero)
-                    - (min_limit - dist).simd_max(zero))
-                    * SimdReal::splat(velocity_based_erp_inv_dt);
-
-                limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
-                    .simd_max(limits_impulse_limits.0)
-                    .simd_min(limits_impulse_limits.1);
-
-                limits_inv_lhs = SimdReal::splat(1.0)
-                    / (im1
-                        + im2
-                        + gcross1.gdot(ii1.transform_vector(gcross1))
-                        + gcross2.gdot(ii2.transform_vector(gcross2)));
-            }
-        }
-
-        WPrismaticVelocityConstraint {
-            joint_id,
-            mj_lambda1,
-            mj_lambda2,
-            im1,
-            ii1_sqrt,
-            im2,
-            ii2_sqrt,
-            limits_active,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
-            limits_forcedir2,
-            limits_rhs,
-            limits_inv_lhs,
-            limits_impulse_limits,
-            basis1,
-            inv_lhs,
-            rhs,
-            r1,
-            r2,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse.y;
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<3>(2).into_owned();
-
-        mj_lambda1.linear += lin_impulse * self.im1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        // Warmstart limits.
-        if self.limits_active {
-            let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse;
-            let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse;
-
-            mj_lambda1.linear += limit_impulse1 * self.im1;
-            mj_lambda1.angular += self
-                .ii1_sqrt
-                .transform_vector(self.r1.gcross(limit_impulse1));
-            mj_lambda2.linear += limit_impulse2 * self.im2;
-            mj_lambda2.angular += self
-                .ii2_sqrt
-                .transform_vector(self.r2.gcross(limit_impulse2));
-        }
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    fn solve_dofs(
-        &mut self,
-        mj_lambda1: &mut DeltaVel<SimdReal>,
-        mj_lambda2: &mut DeltaVel<SimdReal>,
-    ) {
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
-        let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1));
-        let ang_dvel = ang_vel2 - ang_vel1;
-        #[cfg(feature = "dim2")]
-        let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse.y;
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<3>(2).into_owned();
-
-        mj_lambda1.linear += lin_impulse * self.im1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-    }
-
-    fn solve_limits(
-        &mut self,
-        mj_lambda1: &mut DeltaVel<SimdReal>,
-        mj_lambda2: &mut DeltaVel<SimdReal>,
-    ) {
-        if self.limits_active {
-            let limits_forcedir1 = -self.limits_forcedir2;
-            let limits_forcedir2 = self.limits_forcedir2;
-
-            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
-                + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
-                + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs)
-                .simd_max(self.limits_impulse_limits.0)
-                .simd_min(self.limits_impulse_limits.1);
-            let dimpulse = new_impulse - self.limits_impulse;
-            self.limits_impulse = new_impulse;
-
-            let lin_impulse1 = limits_forcedir1 * dimpulse;
-            let lin_impulse2 = limits_forcedir2 * dimpulse;
-
-            mj_lambda1.linear += lin_impulse1 * self.im1;
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
-            mj_lambda2.linear += lin_impulse2 * self.im2;
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
-        }
-    }
-
-    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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
-        self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::PrismaticJoint(rev) = &mut joint.params {
-                rev.impulse = self.impulse.extract(ii);
-                rev.limits_impulse = self.limits_impulse.extract(ii);
-            }
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WPrismaticVelocityGroundConstraint {
-    mj_lambda2: [usize; SIMD_WIDTH],
-
-    joint_id: [JointIndex; SIMD_WIDTH],
-
-    r2: Vector<SimdReal>,
-
-    #[cfg(feature = "dim2")]
-    inv_lhs: Matrix2<SimdReal>,
-    #[cfg(feature = "dim2")]
-    rhs: Vector2<SimdReal>,
-    #[cfg(feature = "dim2")]
-    impulse: Vector2<SimdReal>,
-
-    #[cfg(feature = "dim3")]
-    inv_lhs: Matrix5<SimdReal>,
-    #[cfg(feature = "dim3")]
-    rhs: Vector5<SimdReal>,
-    #[cfg(feature = "dim3")]
-    impulse: Vector5<SimdReal>,
-
-    limits_active: bool,
-    limits_forcedir2: Vector<SimdReal>,
-    limits_impulse: SimdReal,
-    limits_rhs: SimdReal,
-    limits_impulse_limits: (SimdReal, SimdReal),
-
-    axis2: Vector<SimdReal>,
-    #[cfg(feature = "dim2")]
-    basis1: Vector2<SimdReal>,
-    #[cfg(feature = "dim3")]
-    basis1: Matrix3x2<SimdReal>,
-
-    im2: SimdReal,
-    ii2_sqrt: AngularInertia<SimdReal>,
-}
-
-impl WPrismaticVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&PrismaticJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        let (poss1, vels1, mprops1) = rbs1;
-        let (poss2, vels2, mprops2, ids2) = rbs2;
-
-        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
-        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
-
-        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
-        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
-
-        #[cfg(feature = "dim2")]
-        let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
-        #[cfg(feature = "dim3")]
-        let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
-
-        let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
-            cparams[ii].local_anchor2
-        } else {
-            cparams[ii].local_anchor1
-        }]);
-        let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
-            cparams[ii].local_anchor1
-        } else {
-            cparams[ii].local_anchor2
-        }]);
-        let local_axis1 = Vector::from(gather![|ii| if flipped[ii] {
-            *cparams[ii].local_axis2
-        } else {
-            *cparams[ii].local_axis1
-        }]);
-        let local_axis2 = Vector::from(gather![|ii| if flipped[ii] {
-            *cparams[ii].local_axis1
-        } else {
-            *cparams[ii].local_axis2
-        }]);
-
-        #[cfg(feature = "dim2")]
-        let basis1 = position1
-            * Vector::from(gather![|ii| if flipped[ii] {
-                cparams[ii].basis2[0]
-            } else {
-                cparams[ii].basis1[0]
-            }]);
-        #[cfg(feature = "dim3")]
-        let basis1 = Matrix3x2::from_columns(&[
-            position1
-                * Vector::from(gather![|ii| if flipped[ii] {
-                    cparams[ii].basis2[0]
-                } else {
-                    cparams[ii].basis1[0]
-                }]),
-            position1
-                * Vector::from(gather![|ii| if flipped[ii] {
-                    cparams[ii].basis2[1]
-                } else {
-                    cparams[ii].basis1[1]
-                }]),
-        ]);
-
-        let anchor1 = position1 * local_anchor1;
-        let anchor2 = position2 * local_anchor2;
-        let axis1 = position1 * local_axis1;
-        let axis2 = position2 * local_axis2;
-
-        let ii2 = ii2_sqrt.squared();
-        let r1 = anchor1 - world_com1;
-        let r2 = anchor2 - world_com2;
-        let r2_mat = r2.gcross_matrix();
-
-        #[allow(unused_mut)] // For 2D.
-        let mut lhs;
-
-        #[cfg(feature = "dim3")]
-        {
-            let r2_mat_b1 = r2_mat * basis1;
-
-            lhs = Matrix5::zeros();
-            let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
-            let lhs10 = ii2 * r2_mat_b1;
-            let lhs11 = ii2.into_matrix();
-            lhs.fixed_slice_mut::<2, 2>(0, 0)
-                .copy_from(&lhs00.into_matrix());
-            lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
-            lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
-        }
-
-        #[cfg(feature = "dim2")]
-        {
-            let b2r2 = basis1.dot(&r2_mat);
-            let m11 = im2 + b2r2 * ii2 * b2r2;
-            let m12 = basis1.dot(&r2_mat) * ii2;
-            let m22 = ii2;
-            lhs = SdpMatrix2::new(m11, m12, m22);
-        }
-
-        let anchor_linvel1 = linvel1 + angvel1.gcross(r1);
-        let anchor_linvel2 = linvel2 + angvel2.gcross(r2);
-
-        // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
-        // for which a textbook inverse is still efficient.
-        #[cfg(feature = "dim2")]
-        let inv_lhs = lhs.inverse_unchecked().into_matrix();
-        #[cfg(feature = "dim3")]
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let angvel_err = angvel2 - angvel1;
-
-        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
-
-        #[cfg(feature = "dim2")]
-        let mut rhs = Vector2::new(linvel_err.x, angvel_err) * velocity_solve_fraction;
-        #[cfg(feature = "dim3")]
-        let mut rhs = Vector5::new(
-            linvel_err.x,
-            linvel_err.y,
-            angvel_err.x,
-            angvel_err.y,
-            angvel_err.z,
-        ) * velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
-
-            let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
-
-            let frame1 = position1
-                * Isometry::from(gather![|ii| if flipped[ii] {
-                    cparams[ii].local_frame2()
-                } else {
-                    cparams[ii].local_frame1()
-                }]);
-            let frame2 = position2
-                * Isometry::from(gather![|ii| if flipped[ii] {
-                    cparams[ii].local_frame1()
-                } else {
-                    cparams[ii].local_frame2()
-                }]);
-
-            let ang_err = frame2.rotation * frame1.rotation.inverse();
-
-            #[cfg(feature = "dim2")]
-            {
-                rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
-            }
-
-            #[cfg(feature = "dim3")]
-            {
-                let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
-                rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
-                    * velocity_based_erp_inv_dt;
-            }
-        }
-
-        // Setup limit constraint.
-        let zero: SimdReal = na::zero();
-        let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2
-        let mut limits_active = false;
-        let mut limits_rhs = zero;
-        let mut limits_impulse = zero;
-        let mut limits_impulse_limits = (zero, zero);
-
-        let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
-        if limits_enabled.any() {
-            let danchor = anchor2 - anchor1;
-            let dist = danchor.dot(&axis1);
-
-            // TODO: we should allow predictive constraint activation.
-            let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
-            let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
-
-            let min_enabled = dist.simd_lt(min_limit);
-            let max_enabled = dist.simd_gt(max_limit);
-
-            limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero);
-            limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
-
-            limits_active = (min_enabled | max_enabled).any();
-            if limits_active {
-                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
-                    * velocity_solve_fraction;
-
-                limits_rhs += ((dist - max_limit).simd_max(zero)
-                    - (min_limit - dist).simd_max(zero))
-                    * SimdReal::splat(velocity_based_erp_inv_dt);
-
-                limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
-                    .simd_max(limits_impulse_limits.0)
-                    .simd_min(limits_impulse_limits.1);
-            }
-        }
-
-        WPrismaticVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2,
-            im2,
-            ii2_sqrt,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            limits_active,
-            limits_forcedir2,
-            limits_rhs,
-            limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
-            limits_impulse_limits,
-            basis1,
-            inv_lhs,
-            rhs,
-            r2,
-            axis2,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.impulse.y;
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.impulse.fixed_rows::<3>(2).into_owned();
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * self.limits_impulse);
-
-        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);
-        }
-    }
-
-    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let lin_dvel = self.basis1.tr_mul(&lin_vel2);
-        let ang_dvel = ang_vel2;
-        #[cfg(feature = "dim2")]
-        let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
-        #[cfg(feature = "dim3")]
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
-        #[cfg(feature = "dim2")]
-        let ang_impulse = impulse.y;
-        #[cfg(feature = "dim3")]
-        let ang_impulse = impulse.fixed_rows::<3>(2).into_owned();
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-    }
-
-    fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
-        if self.limits_active {
-            // FIXME: the transformation by ii2_sqrt could be avoided by
-            // reusing some computations above.
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let lin_dvel = self
-                .limits_forcedir2
-                .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
-                + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - lin_dvel / self.im2)
-                .simd_max(self.limits_impulse_limits.0)
-                .simd_min(self.limits_impulse_limits.1);
-            let dimpulse = new_impulse - self.limits_impulse;
-            self.limits_impulse = new_impulse;
-
-            mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse);
-        }
-    }
-
-    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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        self.solve_dofs(&mut mj_lambda2);
-        self.solve_limits(&mut mj_lambda2);
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::PrismaticJoint(rev) = &mut joint.params {
-                rev.impulse = self.impulse.extract(ii);
-                rev.limits_impulse = self.limits_impulse.extract(ii);
-            }
-        }
-    }
-}

src/dynamics/solver/joint_constraint/revolute_position_constraint.rs

@@ -1,295 +0,0 @@
-use crate::dynamics::{
-    IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-};
-use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-use na::Unit;
-
-#[derive(Debug)]
-pub(crate) struct RevolutePositionConstraint {
-    position1: usize,
-    position2: usize,
-
-    local_com1: Point<Real>,
-    local_com2: Point<Real>,
-
-    im1: Real,
-    im2: Real,
-
-    ii1: AngularInertia<Real>,
-    ii2: AngularInertia<Real>,
-
-    ang_inv_lhs: AngularInertia<Real>,
-
-    limits_enabled: bool,
-    limits: [Real; 2],
-
-    motor_last_angle: Real,
-
-    local_anchor1: Point<Real>,
-    local_anchor2: Point<Real>,
-
-    local_axis1: Unit<Vector<Real>>,
-    local_axis2: Unit<Vector<Real>>,
-    local_basis1: [Vector<Real>; 2],
-    local_basis2: [Vector<Real>; 2],
-}
-
-impl RevolutePositionConstraint {
-    pub fn from_params(
-        rb1: (&RigidBodyMassProps, &RigidBodyIds),
-        rb2: (&RigidBodyMassProps, &RigidBodyIds),
-        cparams: &RevoluteJoint,
-    ) -> Self {
-        let (mprops1, ids1) = rb1;
-        let (mprops2, ids2) = rb2;
-
-        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let im1 = mprops1.effective_inv_mass;
-        let im2 = mprops2.effective_inv_mass;
-        let ang_inv_lhs = (ii1 + ii2).inverse();
-
-        Self {
-            im1,
-            im2,
-            ii1,
-            ii2,
-            ang_inv_lhs,
-            local_com1: mprops1.local_mprops.local_com,
-            local_com2: mprops2.local_mprops.local_com,
-            local_anchor1: cparams.local_anchor1,
-            local_anchor2: cparams.local_anchor2,
-            local_axis1: cparams.local_axis1,
-            local_axis2: cparams.local_axis2,
-            position1: ids1.active_set_offset,
-            position2: ids2.active_set_offset,
-            local_basis1: cparams.basis1,
-            local_basis2: cparams.basis2,
-            limits_enabled: cparams.limits_enabled,
-            limits: cparams.limits,
-            motor_last_angle: cparams.motor_last_angle,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position1 = positions[self.position1 as usize];
-        let mut position2 = positions[self.position2 as usize];
-
-        /*
-         * Linear part.
-         */
-        {
-            let anchor1 = position1 * self.local_anchor1;
-            let anchor2 = position2 * self.local_anchor2;
-
-            let r1 = anchor1 - position1 * self.local_com1;
-            let r2 = anchor2 - position2 * self.local_com2;
-
-            // TODO: don't do the "to_matrix".
-            let lhs = (self
-                .ii2
-                .quadform(&r2.gcross_matrix())
-                .add_diagonal(self.im2)
-                + self
-                    .ii1
-                    .quadform(&r1.gcross_matrix())
-                    .add_diagonal(self.im1))
-            .into_matrix();
-            let inv_lhs = lhs.try_inverse().unwrap();
-
-            let delta_tra = anchor2 - anchor1;
-            let lin_error = delta_tra * params.joint_erp;
-            let lin_impulse = inv_lhs * lin_error;
-
-            let rot1 = self.ii1 * r1.gcross(lin_impulse);
-            let rot2 = self.ii2 * r2.gcross(lin_impulse);
-            position1.rotation = Rotation::new(rot1) * position1.rotation;
-            position2.rotation = Rotation::new(-rot2) * position2.rotation;
-            position1.translation.vector += self.im1 * lin_impulse;
-            position2.translation.vector -= self.im2 * lin_impulse;
-        }
-
-        /*
-         * Angular part.
-         */
-        {
-            let axis1 = position1 * self.local_axis1;
-            let axis2 = position2 * self.local_axis2;
-            let delta_rot =
-                Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
-            let ang_error = delta_rot.scaled_axis() * params.joint_erp;
-            let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error);
-
-            position1.rotation =
-                Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
-            position2.rotation =
-                Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
-        }
-
-        /*
-         * Limits part.
-         */
-        if self.limits_enabled {
-            let angle = RevoluteJoint::estimate_motor_angle_from_params(
-                &(position1 * self.local_axis1),
-                &(position1 * self.local_basis1[0]),
-                &(position2 * self.local_basis2[0]),
-                self.motor_last_angle,
-            );
-            let ang_error = (angle - self.limits[1]).max(0.0) - (self.limits[0] - angle).max(0.0);
-
-            if ang_error != 0.0 {
-                let axis2 = position2 * self.local_axis2;
-                let ang_impulse = self
-                    .ang_inv_lhs
-                    .transform_vector(*axis2 * ang_error * params.joint_erp);
-
-                position1.rotation =
-                    Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
-                position2.rotation =
-                    Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
-            }
-        }
-
-        positions[self.position1 as usize] = position1;
-        positions[self.position2 as usize] = position2;
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct RevolutePositionGroundConstraint {
-    position2: usize,
-    local_com2: Point<Real>,
-    im2: Real,
-    ii2: AngularInertia<Real>,
-    anchor1: Point<Real>,
-    local_anchor2: Point<Real>,
-    axis1: Unit<Vector<Real>>,
-    basis1: [Vector<Real>; 2],
-
-    limits_enabled: bool,
-    limits: [Real; 2],
-
-    motor_last_angle: Real,
-
-    local_axis2: Unit<Vector<Real>>,
-    local_basis2: [Vector<Real>; 2],
-}
-
-impl RevolutePositionGroundConstraint {
-    pub fn from_params(
-        rb1: &RigidBodyPosition,
-        rb2: (&RigidBodyMassProps, &RigidBodyIds),
-        cparams: &RevoluteJoint,
-        flipped: bool,
-    ) -> Self {
-        let poss1 = rb1;
-        let (mprops2, ids2) = rb2;
-
-        let anchor1;
-        let local_anchor2;
-        let axis1;
-        let local_axis2;
-        let basis1;
-        let local_basis2;
-
-        if flipped {
-            anchor1 = poss1.next_position * cparams.local_anchor2;
-            local_anchor2 = cparams.local_anchor1;
-            axis1 = poss1.next_position * cparams.local_axis2;
-            local_axis2 = cparams.local_axis1;
-            basis1 = [
-                poss1.next_position * cparams.basis2[0],
-                poss1.next_position * cparams.basis2[1],
-            ];
-            local_basis2 = cparams.basis1;
-        } else {
-            anchor1 = poss1.next_position * cparams.local_anchor1;
-            local_anchor2 = cparams.local_anchor2;
-            axis1 = poss1.next_position * cparams.local_axis1;
-            local_axis2 = cparams.local_axis2;
-            basis1 = [
-                poss1.next_position * cparams.basis1[0],
-                poss1.next_position * cparams.basis1[1],
-            ];
-            local_basis2 = cparams.basis2;
-        };
-
-        Self {
-            anchor1,
-            local_anchor2,
-            im2: mprops2.effective_inv_mass,
-            ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
-            local_com2: mprops2.local_mprops.local_com,
-            axis1,
-            local_axis2,
-            position2: ids2.active_set_offset,
-            basis1,
-            local_basis2,
-            limits_enabled: cparams.limits_enabled,
-            limits: cparams.limits,
-            motor_last_angle: cparams.motor_last_angle,
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        let mut position2 = positions[self.position2 as usize];
-
-        /*
-         * Linear part.
-         */
-        {
-            let anchor2 = position2 * self.local_anchor2;
-
-            let r2 = anchor2 - position2 * self.local_com2;
-            // TODO: don't the the "to_matrix".
-            let lhs = self
-                .ii2
-                .quadform(&r2.gcross_matrix())
-                .add_diagonal(self.im2)
-                .into_matrix();
-            let inv_lhs = lhs.try_inverse().unwrap();
-
-            let delta_tra = anchor2 - self.anchor1;
-            let lin_error = delta_tra * params.joint_erp;
-            let lin_impulse = inv_lhs * lin_error;
-
-            let rot2 = self.ii2 * r2.gcross(lin_impulse);
-            position2.rotation = Rotation::new(-rot2) * position2.rotation;
-            position2.translation.vector -= self.im2 * lin_impulse;
-        }
-
-        /*
-         * Angular part.
-         */
-        {
-            let axis2 = position2 * self.local_axis2;
-            let delta_rot = Rotation::rotation_between_axis(&self.axis1, &axis2)
-                .unwrap_or_else(Rotation::identity);
-            let ang_error = delta_rot.scaled_axis() * params.joint_erp;
-            position2.rotation = Rotation::new(-ang_error) * position2.rotation;
-        }
-
-        /*
-         * Limits part.
-         */
-        if self.limits_enabled {
-            let angle = RevoluteJoint::estimate_motor_angle_from_params(
-                &self.axis1,
-                &self.basis1[0],
-                &(position2 * self.local_basis2[0]),
-                self.motor_last_angle,
-            );
-            let ang_error = (angle - self.limits[1]).max(0.0) - (self.limits[0] - angle).max(0.0);
-
-            if ang_error != 0.0 {
-                let axis2 = position2 * self.local_axis2;
-                position2.rotation =
-                    Rotation::new(*axis2 * -ang_error * params.joint_erp) * position2.rotation;
-            }
-        }
-
-        positions[self.position2 as usize] = position2;
-    }
-}

src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs

@@ -1,71 +0,0 @@
-use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
-use crate::dynamics::{
-    IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-};
-use crate::math::{Isometry, Real, SIMD_WIDTH};
-
-// TODO: this does not uses SIMD optimizations yet.
-#[derive(Debug)]
-pub(crate) struct WRevolutePositionConstraint {
-    constraints: [RevolutePositionConstraint; SIMD_WIDTH],
-}
-
-impl WRevolutePositionConstraint {
-    pub fn from_params(
-        rbs1: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&RevoluteJoint; SIMD_WIDTH],
-    ) -> Self {
-        Self {
-            constraints: gather![|ii| RevolutePositionConstraint::from_params(
-                (rbs1.0[ii], rbs1.1[ii]),
-                (rbs2.0[ii], rbs2.1[ii]),
-                cparams[ii]
-            )],
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        for constraint in &self.constraints {
-            constraint.solve(params, positions);
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WRevolutePositionGroundConstraint {
-    constraints: [RevolutePositionGroundConstraint; SIMD_WIDTH],
-}
-
-impl WRevolutePositionGroundConstraint {
-    pub fn from_params(
-        rbs1: [&RigidBodyPosition; SIMD_WIDTH],
-        rbs2: (
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        cparams: [&RevoluteJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        Self {
-            constraints: gather![|ii| RevolutePositionGroundConstraint::from_params(
-                rbs1[ii],
-                (rbs2.0[ii], rbs2.1[ii]),
-                cparams[ii],
-                flipped[ii]
-            )],
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        for constraint in &self.constraints {
-            constraint.solve(params, positions);
-        }
-    }
-}

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -1,750 +0,0 @@
-use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel};
-use crate::dynamics::{
-    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-};
-use crate::math::{AngularInertia, Real, Rotation, Vector};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-use na::{Cholesky, Matrix3x2, Matrix5, UnitQuaternion, Vector5};
-
-#[derive(Debug)]
-pub(crate) struct RevoluteVelocityConstraint {
-    mj_lambda1: usize,
-    mj_lambda2: usize,
-
-    joint_id: JointIndex,
-
-    r1: Vector<Real>,
-    r2: Vector<Real>,
-
-    inv_lhs: Matrix5<Real>,
-    rhs: Vector5<Real>,
-    impulse: Vector5<Real>,
-
-    motor_inv_lhs: Real,
-    motor_rhs: Real,
-    motor_impulse: Real,
-    motor_max_impulse: Real,
-    motor_angle: Real, // Exists only to write it back into the joint.
-
-    motor_axis1: Vector<Real>,
-    motor_axis2: Vector<Real>,
-
-    limits_active: bool,
-    limits_impulse: Real,
-    limits_rhs: Real,
-    limits_inv_lhs: Real,
-    limits_impulse_limits: (Real, Real),
-
-    basis1: Matrix3x2<Real>,
-    basis2: Matrix3x2<Real>,
-
-    im1: Real,
-    im2: Real,
-
-    ii1_sqrt: AngularInertia<Real>,
-    ii2_sqrt: AngularInertia<Real>,
-}
-
-impl RevoluteVelocityConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        rb2: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        joint: &RevoluteJoint,
-    ) -> Self {
-        let (poss1, vels1, mprops1, ids1) = rb1;
-        let (poss2, vels2, mprops2, ids2) = rb2;
-
-        // Linear part.
-        let anchor1 = poss1.position * joint.local_anchor1;
-        let anchor2 = poss2.position * joint.local_anchor2;
-        let basis1 = Matrix3x2::from_columns(&[
-            poss1.position * joint.basis1[0],
-            poss1.position * joint.basis1[1],
-        ]);
-
-        let basis2 = Matrix3x2::from_columns(&[
-            poss2.position * joint.basis2[0],
-            poss2.position * joint.basis2[1],
-        ]);
-        let basis_projection2 = basis2 * basis2.transpose();
-        let basis2 = basis_projection2 * basis1;
-
-        let im1 = mprops1.effective_inv_mass;
-        let im2 = mprops2.effective_inv_mass;
-
-        let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1 - mprops1.world_com;
-        let r1_mat = r1.gcross_matrix();
-
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let r2 = anchor2 - mprops2.world_com;
-        let r2_mat = r2.gcross_matrix();
-
-        let mut lhs = Matrix5::zeros();
-
-        let lhs00 =
-            ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
-        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)) + basis1.tr_mul(&(ii1 * r1_mat));
-        let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix();
-
-        // Note that Cholesky won't read the upper-right part
-        // of lhs so we don't have to fill it.
-        lhs.fixed_slice_mut::<3, 3>(0, 0)
-            .copy_from(&lhs00.into_matrix());
-        lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
-        lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
-
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let linvel_err =
-            (vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
-        let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
-
-        let mut rhs = Vector5::new(
-            linvel_err.x,
-            linvel_err.y,
-            linvel_err.z,
-            angvel_err.x,
-            angvel_err.y,
-        ) * params.velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let lin_err = anchor2 - anchor1;
-
-            let axis1 = poss1.position * joint.local_axis1;
-            let axis2 = poss2.position * joint.local_axis2;
-
-            let axis_error = axis1.cross(&axis2);
-            let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5;
-
-            rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
-                * velocity_based_erp_inv_dt;
-        }
-
-        /*
-         * Motor.
-         */
-        let motor_axis1 = poss1.position * *joint.local_axis1;
-        let motor_axis2 = poss2.position * *joint.local_axis2;
-        let mut motor_rhs = 0.0;
-        let mut motor_inv_lhs = 0.0;
-        let mut motor_angle = 0.0;
-        let motor_max_impulse = joint.motor_max_impulse;
-
-        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
-            params.dt,
-            joint.motor_stiffness,
-            joint.motor_damping,
-        );
-
-        if stiffness != 0.0 || joint.limits_enabled {
-            motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
-        }
-
-        if stiffness != 0.0 {
-            motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
-        }
-
-        if damping != 0.0 {
-            let curr_vel = vels2.angvel.dot(&motor_axis2) - vels1.angvel.dot(&motor_axis1);
-            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
-        }
-
-        if stiffness != 0.0 || damping != 0.0 {
-            motor_inv_lhs = if keep_lhs {
-                crate::utils::inv(
-                    motor_axis2.dot(&ii2.transform_vector(motor_axis2))
-                        + motor_axis1.dot(&ii1.transform_vector(motor_axis1)),
-                ) * gamma
-            } else {
-                gamma
-            };
-            motor_rhs /= gamma;
-        }
-
-        /*
-         * Setup limit constraint.
-         */
-        let mut limits_active = false;
-        let mut limits_rhs = 0.0;
-        let mut limits_inv_lhs = 0.0;
-        let mut limits_impulse_limits = (0.0, 0.0);
-        let mut limits_impulse = 0.0;
-
-        if joint.limits_enabled {
-            // TODO: we should allow predictive constraint activation.
-
-            let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
-            let min_enabled = motor_angle < min_limit;
-            let max_enabled = max_limit < motor_angle;
-
-            limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
-            limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
-            limits_active = min_enabled || max_enabled;
-
-            if limits_active {
-                limits_rhs = (vels2.angvel.dot(&motor_axis2) - vels1.angvel.dot(&motor_axis1))
-                    * params.velocity_solve_fraction;
-
-                limits_rhs += ((motor_angle - max_limit).max(0.0)
-                    - (min_limit - motor_angle).max(0.0))
-                    * velocity_based_erp_inv_dt;
-
-                limits_inv_lhs = crate::utils::inv(
-                    motor_axis2.dot(&ii2.transform_vector(motor_axis2))
-                        + motor_axis1.dot(&ii1.transform_vector(motor_axis1)),
-                );
-
-                limits_impulse = joint
-                    .limits_impulse
-                    .max(limits_impulse_limits.0)
-                    .min(limits_impulse_limits.1)
-                    * params.warmstart_coeff;
-            }
-        }
-
-        /*
-         * Adjust the warmstart impulse.
-         * If the velocity along the free axis is somewhat high,
-         * we need to adjust the angular warmstart impulse because it
-         * may have a direction that is too different than last frame,
-         * making it counter-productive.
-         */
-        let mut impulse = joint.impulse * params.warmstart_coeff;
-        let axis_rot = Rotation::rotation_between(&joint.prev_axis1, &motor_axis1)
-            .unwrap_or_else(UnitQuaternion::identity);
-        let rotated_impulse = basis1.tr_mul(&(axis_rot * joint.world_ang_impulse));
-        impulse[3] = rotated_impulse.x * params.warmstart_coeff;
-        impulse[4] = rotated_impulse.y * params.warmstart_coeff;
-        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
-            * params.warmstart_coeff;
-
-        RevoluteVelocityConstraint {
-            joint_id,
-            mj_lambda1: ids1.active_set_offset,
-            mj_lambda2: ids2.active_set_offset,
-            im1,
-            ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
-            basis1,
-            basis2,
-            im2,
-            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
-            impulse,
-            inv_lhs,
-            rhs,
-            r1,
-            r2,
-            motor_rhs,
-            motor_inv_lhs,
-            motor_max_impulse,
-            motor_axis1,
-            motor_axis2,
-            motor_impulse,
-            motor_angle,
-            limits_impulse,
-            limits_impulse_limits,
-            limits_active,
-            limits_inv_lhs,
-            limits_rhs,
-        }
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
-        let lin_impulse2 = self.impulse.fixed_rows::<3>(0).into_owned();
-        let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<2>(3).into_owned();
-        let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
-
-        mj_lambda1.linear += self.im1 * lin_impulse1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
-
-        mj_lambda2.linear -= self.im2 * lin_impulse2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
-
-        /*
-         * Warmstart motor.
-         */
-        if self.motor_inv_lhs != 0.0 {
-            mj_lambda1.angular += self
-                .ii1_sqrt
-                .transform_vector(self.motor_axis1 * self.motor_impulse);
-            mj_lambda2.angular -= self
-                .ii2_sqrt
-                .transform_vector(self.motor_axis2 * self.motor_impulse);
-        }
-
-        /*
-         * Warmstart limits.
-         */
-        if self.limits_active {
-            let limit_impulse1 = -self.motor_axis2 * self.limits_impulse;
-            let limit_impulse2 = self.motor_axis2 * self.limits_impulse;
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(limit_impulse1);
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
-        }
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-        let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
-            - (mj_lambda1.linear + ang_vel1.gcross(self.r1));
-        let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse1 = impulse.fixed_rows::<3>(0).into_owned();
-        let lin_impulse2 = impulse.fixed_rows::<3>(0).into_owned();
-        let ang_impulse1 = self.basis1 * impulse.fixed_rows::<2>(3).into_owned();
-        let ang_impulse2 = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
-
-        mj_lambda1.linear += self.im1 * lin_impulse1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
-
-        mj_lambda2.linear -= self.im2 * lin_impulse2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
-    }
-
-    fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.limits_active {
-            let limits_torquedir1 = -self.motor_axis2;
-            let limits_torquedir2 = self.motor_axis2;
-
-            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let ang_dvel = limits_torquedir1.dot(&ang_vel1)
-                + limits_torquedir2.dot(&ang_vel2)
-                + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs)
-                .max(self.limits_impulse_limits.0)
-                .min(self.limits_impulse_limits.1);
-            let dimpulse = new_impulse - self.limits_impulse;
-            self.limits_impulse = new_impulse;
-
-            let ang_impulse1 = limits_torquedir1 * dimpulse;
-            let ang_impulse2 = limits_torquedir2 * dimpulse;
-
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(ang_impulse1);
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
-        }
-    }
-
-    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.motor_inv_lhs != 0.0 {
-            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-            let ang_dvel = ang_vel2.dot(&self.motor_axis2) - ang_vel1.dot(&self.motor_axis1);
-            let rhs = ang_dvel + self.motor_rhs;
-
-            let new_motor_impulse = na::clamp(
-                self.motor_impulse + self.motor_inv_lhs * rhs,
-                -self.motor_max_impulse,
-                self.motor_max_impulse,
-            );
-            let impulse = new_motor_impulse - self.motor_impulse;
-            self.motor_impulse = new_motor_impulse;
-
-            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.motor_axis1 * impulse);
-            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
-        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
-        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
-
-        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
-            revolute.impulse = self.impulse;
-            let rot_part = self.impulse.fixed_rows::<2>(3).into_owned();
-            revolute.world_ang_impulse = self.basis1 * rot_part;
-            revolute.prev_axis1 = self.motor_axis1;
-            revolute.motor_last_angle = self.motor_angle;
-            revolute.motor_impulse = self.motor_impulse;
-            revolute.limits_impulse = self.limits_impulse;
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct RevoluteVelocityGroundConstraint {
-    mj_lambda2: usize,
-
-    joint_id: JointIndex,
-
-    r2: Vector<Real>,
-
-    inv_lhs: Matrix5<Real>,
-    rhs: Vector5<Real>,
-    impulse: Vector5<Real>,
-
-    motor_axis2: Vector<Real>,
-    motor_inv_lhs: Real,
-    motor_rhs: Real,
-    motor_impulse: Real,
-    motor_max_impulse: Real,
-    motor_angle: Real, // Exists just for writing it into the joint.
-
-    limits_active: bool,
-    limits_impulse: Real,
-    limits_rhs: Real,
-    limits_inv_lhs: Real,
-    limits_impulse_limits: (Real, Real),
-
-    basis2: Matrix3x2<Real>,
-
-    im2: Real,
-
-    ii2_sqrt: AngularInertia<Real>,
-}
-
-impl RevoluteVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
-        rb2: (
-            &RigidBodyPosition,
-            &RigidBodyVelocity,
-            &RigidBodyMassProps,
-            &RigidBodyIds,
-        ),
-        joint: &RevoluteJoint,
-        flipped: bool,
-    ) -> AnyJointVelocityConstraint {
-        let (poss1, vels1, mprops1) = rb1;
-        let (poss2, vels2, mprops2, ids2) = rb2;
-
-        let anchor2;
-        let anchor1;
-        let axis1;
-        let axis2;
-        let basis1;
-        let basis2;
-
-        if flipped {
-            axis1 = poss1.position * *joint.local_axis2;
-            axis2 = poss2.position * *joint.local_axis1;
-            anchor1 = poss1.position * joint.local_anchor2;
-            anchor2 = poss2.position * joint.local_anchor1;
-            basis1 = Matrix3x2::from_columns(&[
-                poss1.position * joint.basis2[0],
-                poss1.position * joint.basis2[1],
-            ]);
-            basis2 = Matrix3x2::from_columns(&[
-                poss2.position * joint.basis1[0],
-                poss2.position * joint.basis1[1],
-            ]);
-        } else {
-            axis1 = poss1.position * *joint.local_axis1;
-            axis2 = poss2.position * *joint.local_axis2;
-            anchor1 = poss1.position * joint.local_anchor1;
-            anchor2 = poss2.position * joint.local_anchor2;
-            basis1 = Matrix3x2::from_columns(&[
-                poss1.position * joint.basis1[0],
-                poss1.position * joint.basis1[1],
-            ]);
-            basis2 = Matrix3x2::from_columns(&[
-                poss2.position * joint.basis2[0],
-                poss2.position * joint.basis2[1],
-            ]);
-        };
-
-        let basis_projection2 = basis2 * basis2.transpose();
-        let basis2 = basis_projection2 * basis1;
-        let im2 = mprops2.effective_inv_mass;
-        let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
-        let r1 = anchor1 - mprops1.world_com;
-        let r2 = anchor2 - mprops2.world_com;
-        let r2_mat = r2.gcross_matrix();
-
-        let mut lhs = Matrix5::zeros();
-        let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
-        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat));
-        let lhs11 = ii2.quadform3x2(&basis2).into_matrix();
-
-        // Note that cholesky won't read the upper-right part
-        // of lhs so we don't have to fill it.
-        lhs.fixed_slice_mut::<3, 3>(0, 0)
-            .copy_from(&lhs00.into_matrix());
-        lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
-        lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
-
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let linvel_err =
-            (vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
-        let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
-        let mut rhs = Vector5::new(
-            linvel_err.x,
-            linvel_err.y,
-            linvel_err.z,
-            angvel_err.x,
-            angvel_err.y,
-        ) * params.velocity_solve_fraction;
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let lin_err = anchor2 - anchor1;
-
-            let (axis1, axis2);
-            if flipped {
-                axis1 = poss1.position * joint.local_axis2;
-                axis2 = poss2.position * joint.local_axis1;
-            } else {
-                axis1 = poss1.position * joint.local_axis1;
-                axis2 = poss2.position * joint.local_axis2;
-            }
-            let axis_error = axis1.cross(&axis2);
-            let ang_err = basis2.tr_mul(&axis_error);
-
-            rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
-                * velocity_based_erp_inv_dt;
-        }
-
-        /*
-         * Motor part.
-         */
-        let mut motor_rhs = 0.0;
-        let mut motor_inv_lhs = 0.0;
-        let mut motor_angle = 0.0;
-        let motor_max_impulse = joint.motor_max_impulse;
-
-        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
-            params.dt,
-            joint.motor_stiffness,
-            joint.motor_damping,
-        );
-
-        if stiffness != 0.0 || joint.limits_enabled {
-            motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
-        }
-
-        if stiffness != 0.0 {
-            motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
-        }
-
-        if damping != 0.0 {
-            let curr_vel = vels2.angvel.dot(&axis2) - vels1.angvel.dot(&axis1);
-            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
-        }
-
-        if stiffness != 0.0 || damping != 0.0 {
-            motor_inv_lhs = if keep_lhs {
-                crate::utils::inv(axis2.dot(&ii2.transform_vector(axis2))) * gamma
-            } else {
-                gamma
-            };
-            motor_rhs /= gamma;
-        }
-
-        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
-            * params.warmstart_coeff;
-
-        /*
-         * Setup limit constraint.
-         */
-        let mut limits_active = false;
-        let mut limits_rhs = 0.0;
-        let mut limits_inv_lhs = 0.0;
-        let mut limits_impulse_limits = (0.0, 0.0);
-        let mut limits_impulse = 0.0;
-
-        if joint.limits_enabled {
-            // TODO: we should allow predictive constraint activation.
-            let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
-            let min_enabled = motor_angle < min_limit;
-            let max_enabled = max_limit < motor_angle;
-
-            limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
-            limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
-            limits_active = min_enabled || max_enabled;
-
-            if limits_active {
-                limits_rhs = (vels2.angvel.dot(&axis2) - vels1.angvel.dot(&axis1))
-                    * params.velocity_solve_fraction;
-
-                limits_rhs += ((motor_angle - max_limit).max(0.0)
-                    - (min_limit - motor_angle).max(0.0))
-                    * velocity_based_erp_inv_dt;
-
-                limits_inv_lhs = crate::utils::inv(axis2.dot(&ii2.transform_vector(axis2)));
-
-                limits_impulse = joint
-                    .limits_impulse
-                    .max(limits_impulse_limits.0)
-                    .min(limits_impulse_limits.1)
-                    * params.warmstart_coeff;
-            }
-        }
-
-        let result = RevoluteVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2: ids2.active_set_offset,
-            im2,
-            ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
-            impulse: joint.impulse * params.warmstart_coeff,
-            basis2,
-            inv_lhs,
-            rhs,
-            r2,
-            motor_inv_lhs,
-            motor_impulse,
-            motor_axis2: axis2,
-            motor_max_impulse,
-            motor_rhs,
-            motor_angle,
-            limits_impulse,
-            limits_impulse_limits,
-            limits_active,
-            limits_inv_lhs,
-            limits_rhs,
-        };
-
-        AnyJointVelocityConstraint::RevoluteGroundConstraint(result)
-    }
-
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned();
-        let ang_impulse = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        /*
-         * Motor
-         */
-        if self.motor_inv_lhs != 0.0 {
-            mj_lambda2.angular -= self
-                .ii2_sqrt
-                .transform_vector(self.motor_axis2 * self.motor_impulse);
-        }
-
-        // Warmstart limits.
-        if self.limits_active {
-            let limit_impulse2 = self.motor_axis2 * self.limits_impulse;
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
-        }
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-        let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let ang_dvel = self.basis2.tr_mul(&ang_vel2);
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<3>(0).into_owned();
-        let ang_impulse = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-    }
-
-    fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.limits_active {
-            let limits_torquedir2 = self.motor_axis2;
-
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-            let ang_dvel = limits_torquedir2.dot(&ang_vel2) + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs)
-                .max(self.limits_impulse_limits.0)
-                .min(self.limits_impulse_limits.1);
-            let dimpulse = new_impulse - self.limits_impulse;
-            self.limits_impulse = new_impulse;
-
-            let ang_impulse2 = limits_torquedir2 * dimpulse;
-            mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
-        }
-    }
-
-    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        if self.motor_inv_lhs != 0.0 {
-            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-            let ang_dvel = ang_vel2.dot(&self.motor_axis2);
-            let rhs = ang_dvel + self.motor_rhs;
-
-            let new_motor_impulse = na::clamp(
-                self.motor_impulse + self.motor_inv_lhs * rhs,
-                -self.motor_max_impulse,
-                self.motor_max_impulse,
-            );
-            let impulse = new_motor_impulse - self.motor_impulse;
-            self.motor_impulse = new_motor_impulse;
-
-            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        self.solve_limits(&mut mj_lambda2);
-        self.solve_dofs(&mut mj_lambda2);
-        self.solve_motors(&mut mj_lambda2);
-
-        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
-    }
-
-    // FIXME: duplicated code with the non-ground constraint.
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let joint = &mut joints_all[self.joint_id].weight;
-        if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
-            revolute.impulse = self.impulse;
-            revolute.motor_impulse = self.motor_impulse;
-            revolute.motor_last_angle = self.motor_angle;
-            revolute.limits_impulse = self.limits_impulse;
-        }
-    }
-}

src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs

@@ -1,527 +0,0 @@
-use simba::simd::SimdValue;
-
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-};
-use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
-};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-use na::{Cholesky, Matrix3x2, Matrix5, Unit, Vector5};
-
-#[derive(Debug)]
-pub(crate) struct WRevoluteVelocityConstraint {
-    mj_lambda1: [usize; SIMD_WIDTH],
-    mj_lambda2: [usize; SIMD_WIDTH],
-
-    joint_id: [JointIndex; SIMD_WIDTH],
-
-    r1: Vector<SimdReal>,
-    r2: Vector<SimdReal>,
-
-    inv_lhs: Matrix5<SimdReal>,
-    rhs: Vector5<SimdReal>,
-    impulse: Vector5<SimdReal>,
-
-    axis1: [Vector<Real>; SIMD_WIDTH],
-    basis1: Matrix3x2<SimdReal>,
-    basis2: Matrix3x2<SimdReal>,
-
-    im1: SimdReal,
-    im2: SimdReal,
-
-    ii1_sqrt: AngularInertia<SimdReal>,
-    ii2_sqrt: AngularInertia<SimdReal>,
-}
-
-impl WRevoluteVelocityConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        joints: [&RevoluteJoint; SIMD_WIDTH],
-    ) -> Self {
-        let (poss1, vels1, mprops1, ids1) = rbs1;
-        let (poss2, vels2, mprops2, ids2) = rbs2;
-
-        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
-        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
-        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
-        let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops1[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
-
-        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
-        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
-
-        let local_anchor1 = Point::from(gather![|ii| joints[ii].local_anchor1]);
-        let local_anchor2 = Point::from(gather![|ii| joints[ii].local_anchor2]);
-        let local_basis1 = [
-            Vector::from(gather![|ii| joints[ii].basis1[0]]),
-            Vector::from(gather![|ii| joints[ii].basis1[1]]),
-        ];
-        let local_basis2 = [
-            Vector::from(gather![|ii| joints[ii].basis2[0]]),
-            Vector::from(gather![|ii| joints[ii].basis2[1]]),
-        ];
-        let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
-
-        let anchor1 = position1 * local_anchor1;
-        let anchor2 = position2 * local_anchor2;
-        let basis1 =
-            Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
-        let basis2 =
-            Matrix3x2::from_columns(&[position2 * local_basis2[0], position2 * local_basis2[1]]);
-        let basis_projection2 = basis2 * basis2.transpose();
-        let basis2 = basis_projection2 * basis1;
-
-        let ii1 = ii1_sqrt.squared();
-        let r1 = anchor1 - world_com1;
-        let r1_mat = r1.gcross_matrix();
-
-        let ii2 = ii2_sqrt.squared();
-        let r2 = anchor2 - world_com2;
-        let r2_mat = r2.gcross_matrix();
-
-        let mut lhs = Matrix5::zeros();
-        let lhs00 =
-            ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)) + basis2.tr_mul(&(ii1 * r1_mat));
-        let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix();
-
-        // Note that Cholesky won't read the upper-right part
-        // of lhs so we don't have to fill it.
-        lhs.fixed_slice_mut::<3, 3>(0, 0)
-            .copy_from(&lhs00.into_matrix());
-        lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
-        lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
-
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let linvel_err = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
-        let angvel_err = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
-
-        let mut rhs = Vector5::new(
-            linvel_err.x,
-            linvel_err.y,
-            linvel_err.z,
-            angvel_err.x,
-            angvel_err.y,
-        ) * SimdReal::splat(params.velocity_solve_fraction);
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
-
-            let lin_err = anchor2 - anchor1;
-
-            let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis1]);
-            let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis2]);
-
-            let axis1 = position1 * local_axis1;
-            let axis2 = position2 * local_axis2;
-
-            let axis_error = axis1.cross(&axis2);
-            let ang_err =
-                (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * SimdReal::splat(0.5);
-
-            rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
-                * velocity_based_erp_inv_dt;
-        }
-
-        /*
-         * Adjust the warmstart impulse.
-         * If the velocity along the free axis is somewhat high,
-         * we need to adjust the angular warmstart impulse because it
-         * may have a direction that is too different than last frame,
-         * making it counter-productive.
-         */
-        let warmstart_coeff = SimdReal::splat(params.warmstart_coeff);
-        let mut impulse = impulse * warmstart_coeff;
-
-        let axis1 = gather![|ii| poss1[ii].position * *joints[ii].local_axis1];
-        let rotated_impulse = Vector::from(gather![|ii| {
-            let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii])
-                .unwrap_or_else(Rotation::identity);
-            axis_rot * joints[ii].world_ang_impulse
-        }]);
-
-        let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse);
-        impulse[3] = rotated_basis_impulse.x * warmstart_coeff;
-        impulse[4] = rotated_basis_impulse.y * warmstart_coeff;
-
-        WRevoluteVelocityConstraint {
-            joint_id,
-            mj_lambda1,
-            mj_lambda2,
-            im1,
-            ii1_sqrt,
-            axis1,
-            basis1,
-            basis2,
-            im2,
-            ii2_sqrt,
-            impulse,
-            inv_lhs,
-            rhs,
-            r1,
-            r2,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
-        let lin_impulse2 = self.impulse.fixed_rows::<3>(0).into_owned();
-        let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<2>(3).into_owned();
-        let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
-
-        mj_lambda1.linear += lin_impulse1 * self.im1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
-
-        mj_lambda2.linear -= lin_impulse2 * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-
-        let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
-            - (mj_lambda1.linear + ang_vel1.gcross(self.r1));
-        let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse1 = impulse.fixed_rows::<3>(0).into_owned();
-        let lin_impulse2 = impulse.fixed_rows::<3>(0).into_owned();
-        let ang_impulse1 = self.basis1 * impulse.fixed_rows::<2>(3).into_owned();
-        let ang_impulse2 = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
-
-        mj_lambda1.linear += lin_impulse1 * self.im1;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
-
-        mj_lambda2.linear -= lin_impulse2 * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        let rot_part = self.impulse.fixed_rows::<2>(3).into_owned();
-        let world_ang_impulse = self.basis1 * rot_part;
-
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::RevoluteJoint(rev) = &mut joint.params {
-                rev.impulse = self.impulse.extract(ii);
-                rev.world_ang_impulse = world_ang_impulse.extract(ii);
-                rev.prev_axis1 = self.axis1[ii];
-            }
-        }
-    }
-}
-
-#[derive(Debug)]
-pub(crate) struct WRevoluteVelocityGroundConstraint {
-    mj_lambda2: [usize; SIMD_WIDTH],
-
-    joint_id: [JointIndex; SIMD_WIDTH],
-
-    r2: Vector<SimdReal>,
-
-    inv_lhs: Matrix5<SimdReal>,
-    rhs: Vector5<SimdReal>,
-    impulse: Vector5<SimdReal>,
-
-    basis2: Matrix3x2<SimdReal>,
-
-    im2: SimdReal,
-
-    ii2_sqrt: AngularInertia<SimdReal>,
-}
-
-impl WRevoluteVelocityGroundConstraint {
-    pub fn from_params(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-        ),
-        rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ),
-        joints: [&RevoluteJoint; SIMD_WIDTH],
-        flipped: [bool; SIMD_WIDTH],
-    ) -> Self {
-        let (poss1, vels1, mprops1) = rbs1;
-        let (poss2, vels2, mprops2, ids2) = rbs2;
-
-        let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
-        let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
-
-        let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
-        let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
-            |ii| mprops2[ii].effective_world_inv_inertia_sqrt
-        ]);
-        let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
-        let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
-
-        let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
-            joints[ii].local_anchor2
-        } else {
-            joints[ii].local_anchor1
-        }]);
-        let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
-            joints[ii].local_anchor1
-        } else {
-            joints[ii].local_anchor2
-        }]);
-        let basis1 = Matrix3x2::from_columns(&[
-            position1
-                * Vector::from(gather![|ii| if flipped[ii] {
-                    joints[ii].basis2[0]
-                } else {
-                    joints[ii].basis1[0]
-                }]),
-            position1
-                * Vector::from(gather![|ii| if flipped[ii] {
-                    joints[ii].basis2[1]
-                } else {
-                    joints[ii].basis1[1]
-                }]),
-        ]);
-        let basis2 = Matrix3x2::from_columns(&[
-            position2
-                * Vector::from(gather![|ii| if flipped[ii] {
-                    joints[ii].basis1[0]
-                } else {
-                    joints[ii].basis2[0]
-                }]),
-            position2
-                * Vector::from(gather![|ii| if flipped[ii] {
-                    joints[ii].basis1[1]
-                } else {
-                    joints[ii].basis2[1]
-                }]),
-        ]);
-        let basis_projection2 = basis2 * basis2.transpose();
-        let basis2 = basis_projection2 * basis1;
-
-        let anchor1 = position1 * local_anchor1;
-        let anchor2 = position2 * local_anchor2;
-
-        let ii2 = ii2_sqrt.squared();
-        let r1 = anchor1 - world_com1;
-        let r2 = anchor2 - world_com2;
-        let r2_mat = r2.gcross_matrix();
-
-        let mut lhs = Matrix5::zeros();
-        let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
-        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat));
-        let lhs11 = ii2.quadform3x2(&basis2).into_matrix();
-
-        // Note that cholesky won't read the upper-right part
-        // of lhs so we don't have to fill it.
-        lhs.fixed_slice_mut::<3, 3>(0, 0)
-            .copy_from(&lhs00.into_matrix());
-        lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
-        lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
-
-        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
-
-        let linvel_err = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1));
-        let angvel_err = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
-
-        let mut rhs = Vector5::new(
-            linvel_err.x,
-            linvel_err.y,
-            linvel_err.z,
-            angvel_err.x,
-            angvel_err.y,
-        ) * SimdReal::splat(params.velocity_solve_fraction);
-
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
-        if velocity_based_erp_inv_dt != 0.0 {
-            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
-
-            let lin_err = anchor2 - anchor1;
-
-            let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
-                joints[ii].local_axis2
-            } else {
-                joints[ii].local_axis1
-            }]);
-            let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
-                joints[ii].local_axis1
-            } else {
-                joints[ii].local_axis2
-            }]);
-            let axis1 = position1 * local_axis1;
-            let axis2 = position2 * local_axis2;
-
-            let axis_error = axis1.cross(&axis2);
-            let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error);
-
-            rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
-                * velocity_based_erp_inv_dt;
-        }
-
-        WRevoluteVelocityGroundConstraint {
-            joint_id,
-            mj_lambda2,
-            im2,
-            ii2_sqrt,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            basis2,
-            inv_lhs,
-            rhs,
-            r2,
-        }
-    }
-
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned();
-        let ang_impulse = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let ang_dvel = self.basis2.tr_mul(&ang_vel2);
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<3>(0).into_owned();
-        let ang_impulse = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
-
-        mj_lambda2.linear -= lin_impulse * self.im2;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
-        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);
-        }
-    }
-
-    // FIXME: duplicated code with the non-ground constraint.
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        for ii in 0..SIMD_WIDTH {
-            let joint = &mut joints_all[self.joint_id[ii]].weight;
-            if let JointParams::RevoluteJoint(rev) = &mut joint.params {
-                rev.impulse = self.impulse.extract(ii)
-            }
-        }
-    }
-}

src/dynamics/solver/mod.rs

@@ -3,26 +3,19 @@ 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_position_solver::ParallelPositionSolver;
-#[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::position_solver::PositionSolver;
-#[cfg(not(feature = "parallel"))]
 pub(self) use self::solver_constraints::SolverConstraints;
 #[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 interaction_groups::*;
-pub(self) use joint_constraint::*;
-pub(self) use position_constraint::*;
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use position_constraint_wide::*;
-pub(self) use position_ground_constraint::*;
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use position_ground_constraint_wide::*;
+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")]
@@ -34,6 +27,8 @@ pub(self) use velocity_ground_constraint_wide::*;
 
 mod categorization;
 mod delta_vel;
+mod generic_velocity_constraint;
+mod generic_velocity_constraint_element;
 mod interaction_groups;
 #[cfg(not(feature = "parallel"))]
 mod island_solver;
@@ -41,19 +36,9 @@ mod joint_constraint;
 #[cfg(feature = "parallel")]
 mod parallel_island_solver;
 #[cfg(feature = "parallel")]
-mod parallel_position_solver;
-#[cfg(feature = "parallel")]
 mod parallel_solver_constraints;
 #[cfg(feature = "parallel")]
 mod parallel_velocity_solver;
-mod position_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod position_constraint_wide;
-mod position_ground_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod position_ground_constraint_wide;
-#[cfg(not(feature = "parallel"))]
-mod position_solver;
 #[cfg(not(feature = "parallel"))]
 mod solver_constraints;
 mod velocity_constraint;

src/dynamics/solver/parallel_island_solver.rs

@@ -3,14 +3,14 @@ use std::sync::atomic::{AtomicUsize, Ordering};
 use rayon::Scope;
 
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
+use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint;
 use crate::dynamics::solver::{
-    AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
-    AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints,
+    AnyJointVelocityConstraint, AnyVelocityConstraint, ParallelSolverConstraints,
 };
 use crate::dynamics::{
-    IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, RigidBodyDamping,
-    RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
-    RigidBodyVelocity,
+    IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
+    RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
+    RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Isometry, Real};
@@ -69,8 +69,6 @@ pub(crate) struct ThreadContext {
     pub num_initialized_constraints: AtomicUsize,
     pub joint_constraint_initialization_index: AtomicUsize,
     pub num_initialized_joint_constraints: AtomicUsize,
-    pub warmstart_constraint_index: AtomicUsize,
-    pub num_warmstarted_constraints: AtomicUsize,
     pub solve_interaction_index: AtomicUsize,
     pub num_solved_interactions: AtomicUsize,
     pub impulse_writeback_index: AtomicUsize,
@@ -79,10 +77,6 @@ pub(crate) struct ThreadContext {
     pub body_force_integration_index: AtomicUsize,
     pub num_force_integrated_bodies: AtomicUsize,
     pub num_integrated_bodies: AtomicUsize,
-    // Position solver.
-    pub solve_position_interaction_index: AtomicUsize,
-    pub num_solved_position_interactions: AtomicUsize,
-    pub position_writeback_index: AtomicUsize,
 }
 
 impl ThreadContext {
@@ -93,8 +87,6 @@ impl ThreadContext {
             num_initialized_constraints: AtomicUsize::new(0),
             joint_constraint_initialization_index: AtomicUsize::new(0),
             num_initialized_joint_constraints: AtomicUsize::new(0),
-            num_warmstarted_constraints: AtomicUsize::new(0),
-            warmstart_constraint_index: AtomicUsize::new(0),
             solve_interaction_index: AtomicUsize::new(0),
             num_solved_interactions: AtomicUsize::new(0),
             impulse_writeback_index: AtomicUsize::new(0),
@@ -103,9 +95,6 @@ impl ThreadContext {
             num_force_integrated_bodies: AtomicUsize::new(0),
             body_integration_index: AtomicUsize::new(0),
             num_integrated_bodies: AtomicUsize::new(0),
-            solve_position_interaction_index: AtomicUsize::new(0),
-            num_solved_position_interactions: AtomicUsize::new(0),
-            position_writeback_index: AtomicUsize::new(0),
         }
     }
 
@@ -122,14 +111,13 @@ impl ThreadContext {
 }
 
 pub struct ParallelIslandSolver {
-    mj_lambdas: Vec<DeltaVel<Real>>,
+    velocity_solver: ParallelVelocitySolver,
     positions: Vec<Isometry<Real>>,
     parallel_groups: ParallelInteractionGroups,
     parallel_joint_groups: ParallelInteractionGroups,
     parallel_contact_constraints:
-        ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>,
-    parallel_joint_constraints:
-        ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>,
+        ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>,
+    parallel_joint_constraints: ParallelSolverConstraints<AnyJointVelocityConstraint, ()>,
     thread: ThreadContext,
 }
 
@@ -142,7 +130,7 @@ impl Default for ParallelIslandSolver {
 impl ParallelIslandSolver {
     pub fn new() -> Self {
         Self {
-            mj_lambdas: Vec::new(),
+            velocity_solver: ParallelVelocitySolver::new(),
             positions: Vec::new(),
             parallel_groups: ParallelInteractionGroups::new(),
             parallel_joint_groups: ParallelInteractionGroups::new(),
@@ -152,84 +140,7 @@ impl ParallelIslandSolver {
         }
     }
 
-    pub fn solve_position_constraints<'s, Bodies>(
-        &'s mut self,
-        scope: &Scope<'s>,
-        island_id: usize,
-        islands: &'s IslandManager,
-        params: &'s IntegrationParameters,
-        bodies: &'s mut Bodies,
-    ) where
-        Bodies: ComponentSetMut<RigidBodyPosition> + ComponentSet<RigidBodyIds>,
-    {
-        let num_threads = rayon::current_num_threads();
-        let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
-        self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
-        self.positions.clear();
-        self.positions
-            .resize(islands.active_island(island_id).len(), Isometry::identity());
-
-        for _ in 0..num_task_per_island {
-            // We use AtomicPtr because it is Send+Sync while *mut is not.
-            // See https://internals.rust-lang.org/t/shouldnt-pointers-be-send-sync-or/8818
-            let thread = &self.thread;
-            let positions = std::sync::atomic::AtomicPtr::new(&mut self.positions as *mut _);
-            let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _);
-            let parallel_contact_constraints =
-                std::sync::atomic::AtomicPtr::new(&mut self.parallel_contact_constraints as *mut _);
-            let parallel_joint_constraints =
-                std::sync::atomic::AtomicPtr::new(&mut self.parallel_joint_constraints as *mut _);
-
-            scope.spawn(move |_| {
-                // Transmute *mut -> &mut
-                let positions: &mut Vec<Isometry<Real>> =
-                    unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
-                let bodies: &mut Bodies =
-                    unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
-                let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe {
-                    std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
-                };
-                let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> = unsafe {
-                    std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed))
-                };
-
-                enable_flush_to_zero!(); // Ensure this is enabled on each thread.
-
-                // Write results back to rigid bodies and integrate velocities.
-                let island_range = islands.active_island_range(island_id);
-                let active_bodies = &islands.active_dynamic_set[island_range];
-
-                concurrent_loop! {
-                    let batch_size = thread.batch_size;
-                    for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
-                        let (rb_ids, rb_pos): (&RigidBodyIds, &RigidBodyPosition) = bodies.index_bundle(handle.0);
-                        positions[rb_ids.active_set_offset] = rb_pos.next_position;
-                    }
-                }
-
-                ThreadContext::lock_until_ge(&thread.num_integrated_bodies, active_bodies.len());
-
-                ParallelPositionSolver::solve(
-                    &thread,
-                    params,
-                    positions,
-                    parallel_contact_constraints,
-                    parallel_joint_constraints
-                );
-
-                // Write results back to rigid bodies.
-                concurrent_loop! {
-                    let batch_size = thread.batch_size;
-                    for handle in active_bodies[thread.position_writeback_index] {
-                        let rb_ids: RigidBodyIds = *bodies.index(handle.0);
-                        bodies.map_mut_internal(handle.0, |rb_pos: &mut RigidBodyPosition| rb_pos.next_position = positions[rb_ids.active_set_offset]);
-                    }
-                }
-            })
-        }
-    }
-
-    pub fn init_constraints_and_solve_velocity_constraints<'s, Bodies>(
+    pub fn init_and_solve<'s, Bodies>(
         &'s mut self,
         scope: &Scope<'s>,
         island_id: usize,
@@ -238,8 +149,9 @@ impl ParallelIslandSolver {
         bodies: &'s mut Bodies,
         manifolds: &'s mut Vec<&'s mut ContactManifold>,
         manifold_indices: &'s [ContactManifoldIndex],
-        joints: &'s mut Vec<JointGraphEdge>,
+        impulse_joints: &'s mut Vec<JointGraphEdge>,
         joint_indices: &[JointIndex],
+        multibody_joints: &mut MultibodyJointSet,
     ) where
         Bodies: ComponentSet<RigidBodyForces>
             + ComponentSetMut<RigidBodyPosition>
@@ -263,13 +175,14 @@ impl ParallelIslandSolver {
             island_id,
             islands,
             bodies,
-            joints,
+            impulse_joints,
             joint_indices,
         );
         self.parallel_contact_constraints.init_constraint_groups(
             island_id,
             islands,
             bodies,
+            multibody_joints,
             manifolds,
             &self.parallel_groups,
         );
@@ -277,25 +190,25 @@ impl ParallelIslandSolver {
             island_id,
             islands,
             bodies,
-            joints,
+            multibody_joints,
+            impulse_joints,
             &self.parallel_joint_groups,
         );
 
-        self.mj_lambdas.clear();
-        self.mj_lambdas
+        self.velocity_solver.mj_lambdas.clear();
+        self.velocity_solver
+            .mj_lambdas
             .resize(islands.active_island(island_id).len(), DeltaVel::zero());
-        self.positions.clear();
-        self.positions
-            .resize(islands.active_island(island_id).len(), Isometry::identity());
 
         for _ in 0..num_task_per_island {
             // We use AtomicPtr because it is Send+Sync while *mut is not.
             // See https://internals.rust-lang.org/t/shouldnt-pointers-be-send-sync-or/8818
             let thread = &self.thread;
-            let mj_lambdas = std::sync::atomic::AtomicPtr::new(&mut self.mj_lambdas as *mut _);
+            let velocity_solver =
+                std::sync::atomic::AtomicPtr::new(&mut self.velocity_solver as *mut _);
             let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _);
             let manifolds = std::sync::atomic::AtomicPtr::new(manifolds as *mut _);
-            let joints = std::sync::atomic::AtomicPtr::new(joints as *mut _);
+            let impulse_joints = std::sync::atomic::AtomicPtr::new(impulse_joints as *mut _);
             let parallel_contact_constraints =
                 std::sync::atomic::AtomicPtr::new(&mut self.parallel_contact_constraints as *mut _);
             let parallel_joint_constraints =
@@ -303,18 +216,18 @@ impl ParallelIslandSolver {
 
             scope.spawn(move |_| {
                 // Transmute *mut -> &mut
-                let mj_lambdas: &mut Vec<DeltaVel<Real>> =
-                    unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
+                let velocity_solver: &mut ParallelVelocitySolver =
+                    unsafe { std::mem::transmute(velocity_solver.load(Ordering::Relaxed)) };
                 let bodies: &mut Bodies =
                     unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
                 let manifolds: &mut Vec<&mut ContactManifold> =
                     unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
-                let joints: &mut Vec<JointGraphEdge> =
-                    unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) };
-                let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe {
+                let impulse_joints: &mut Vec<JointGraphEdge> =
+                    unsafe { std::mem::transmute(impulse_joints.load(Ordering::Relaxed)) };
+                let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> = unsafe {
                     std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
                 };
-                let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> = unsafe {
+                let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, ()> = unsafe {
                     std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed))
                 };
 
@@ -329,7 +242,7 @@ impl ParallelIslandSolver {
                         let batch_size = thread.batch_size;
                         for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] {
                             let (rb_ids, rb_forces, rb_mass_props): (&RigidBodyIds, &RigidBodyForces, &RigidBodyMassProps) = bodies.index_bundle(handle.0);
-                            let dvel = &mut mj_lambdas[rb_ids.active_set_offset];
+                            let dvel = &mut velocity_solver.mj_lambdas[rb_ids.active_set_offset];
 
                             // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
                             //       by the square root of the inertia tensor:
@@ -345,7 +258,7 @@ impl ParallelIslandSolver {
 
 
                 parallel_contact_constraints.fill_constraints(&thread, params, bodies, manifolds);
-                parallel_joint_constraints.fill_constraints(&thread, params, bodies, joints);
+                parallel_joint_constraints.fill_constraints(&thread, params, bodies, impulse_joints);
                 ThreadContext::lock_until_ge(
                     &thread.num_initialized_constraints,
                     parallel_contact_constraints.constraint_descs.len(),
@@ -355,14 +268,13 @@ impl ParallelIslandSolver {
                     parallel_joint_constraints.constraint_descs.len(),
                 );
 
-                ParallelVelocitySolver::solve(
+                velocity_solver.solve(
                         &thread,
                         params,
                         manifolds,
-                        joints,
-                        mj_lambdas,
+                        impulse_joints,
                         parallel_contact_constraints,
-                        parallel_joint_constraints
+                        parallel_joint_constraints,
                 );
 
                 // Write results back to rigid bodies and integrate velocities.
@@ -383,7 +295,7 @@ impl ParallelIslandSolver {
                         let mut new_rb_pos = *rb_pos;
                         let mut new_rb_vels = *rb_vels;
 
-                        let dvels = mj_lambdas[rb_ids.active_set_offset];
+                        let dvels = velocity_solver.mj_lambdas[rb_ids.active_set_offset];
                         new_rb_vels.linvel += dvels.linear;
                         new_rb_vels.angvel += rb_mprops.effective_world_inv_inertia_sqrt.transform_vector(dvels.angular);
 

src/dynamics/solver/parallel_position_solver.rs

@@ -1,107 +0,0 @@
-use super::{AnyJointPositionConstraint, AnyPositionConstraint, ThreadContext};
-use crate::dynamics::solver::{
-    AnyJointVelocityConstraint, AnyVelocityConstraint, ParallelSolverConstraints,
-};
-use crate::dynamics::IntegrationParameters;
-use crate::math::{Isometry, Real};
-use std::sync::atomic::Ordering;
-
-pub(crate) struct ParallelPositionSolver;
-
-impl ParallelPositionSolver {
-    pub fn solve(
-        thread: &ThreadContext,
-        params: &IntegrationParameters,
-        positions: &mut [Isometry<Real>],
-        contact_constraints: &mut ParallelSolverConstraints<
-            AnyVelocityConstraint,
-            AnyPositionConstraint,
-        >,
-        joint_constraints: &mut ParallelSolverConstraints<
-            AnyJointVelocityConstraint,
-            AnyJointPositionConstraint,
-        >,
-    ) {
-        if contact_constraints.constraint_descs.is_empty()
-            && joint_constraints.constraint_descs.is_empty()
-        {
-            return;
-        }
-
-        /*
-         * Solve constraints.
-         */
-        {
-            // Each thread will concurrently grab thread.batch_size constraint desc to
-            // solve. If the batch size is large enough for to cross the boundary of
-            // a palallel_desc_group, we have to wait util the current group is finished
-            // before starting the next one.
-            let mut start_index = thread
-                .solve_position_interaction_index
-                .fetch_add(thread.batch_size, Ordering::SeqCst);
-            let mut batch_size = thread.batch_size;
-            let contact_descs = &contact_constraints.constraint_descs[..];
-            let joint_descs = &joint_constraints.constraint_descs[..];
-            let mut target_num_desc = 0;
-            let mut shift = 0;
-
-            for _ in 0..params.max_position_iterations {
-                macro_rules! solve {
-                    ($part: expr) => {
-                        // Joint groups.
-                        for group in $part.parallel_desc_groups.windows(2) {
-                            let num_descs_in_group = group[1] - group[0];
-                            target_num_desc += num_descs_in_group;
-
-                            while start_index < group[1] {
-                                let end_index = (start_index + batch_size).min(group[1]);
-
-                                let constraints = if end_index == $part.constraint_descs.len() {
-                                    &mut $part.position_constraints
-                                        [$part.constraint_descs[start_index].0..]
-                                } else {
-                                    &mut $part.position_constraints[$part.constraint_descs
-                                        [start_index]
-                                        .0
-                                        ..$part.constraint_descs[end_index].0]
-                                };
-
-                                for constraint in constraints {
-                                    constraint.solve(params, positions);
-                                }
-
-                                let num_solved = end_index - start_index;
-                                batch_size -= num_solved;
-
-                                thread
-                                    .num_solved_position_interactions
-                                    .fetch_add(num_solved, Ordering::SeqCst);
-
-                                if batch_size == 0 {
-                                    start_index = thread
-                                        .solve_position_interaction_index
-                                        .fetch_add(thread.batch_size, Ordering::SeqCst);
-                                    start_index -= shift;
-                                    batch_size = thread.batch_size;
-                                } else {
-                                    start_index += num_solved;
-                                }
-                            }
-                            ThreadContext::lock_until_ge(
-                                &thread.num_solved_position_interactions,
-                                target_num_desc,
-                            );
-                        }
-                    };
-                }
-
-                solve!(joint_constraints);
-                shift += joint_descs.len();
-                start_index -= joint_descs.len();
-                solve!(contact_constraints);
-                shift += contact_descs.len();
-                start_index -= contact_descs.len();
-            }
-        }
-    }
-}

src/dynamics/solver/parallel_solver_constraints.rs

@@ -2,23 +2,20 @@ use super::ParallelInteractionGroups;
 use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext};
 use crate::data::ComponentSet;
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
-use crate::dynamics::solver::{
-    AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint,
-    PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint,
-};
+use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint;
+use crate::dynamics::solver::{InteractionGroups, VelocityConstraint, VelocityGroundConstraint};
 use crate::dynamics::{
-    IntegrationParameters, IslandManager, JointGraphEdge, RigidBodyIds, RigidBodyMassProps,
-    RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
+    IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyIds,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::ContactManifold;
+use crate::math::Real;
 #[cfg(feature = "simd-is-enabled")]
 use crate::{
-    dynamics::solver::{
-        WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint,
-        WVelocityGroundConstraint,
-    },
+    dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
     math::SIMD_WIDTH,
 };
+use na::DVector;
 use std::sync::atomic::Ordering;
 
 // pub fn init_constraint_groups(
@@ -27,13 +24,13 @@ use std::sync::atomic::Ordering;
 //     bodies: &impl ComponentSet<RigidBody>,
 //     manifolds: &mut [&mut ContactManifold],
 //     manifold_groups: &ParallelInteractionGroups,
-//     joints: &mut [JointGraphEdge],
+//     impulse_joints: &mut [JointGraphEdge],
 //     joint_groups: &ParallelInteractionGroups,
 // ) {
 //     self.part
 //         .init_constraints_groups(island_id, bodies, manifolds, manifold_groups);
 //     self.joint_part
-//         .init_constraints_groups(island_id, bodies, joints, joint_groups);
+//         .init_constraints_groups(island_id, bodies, impulse_joints, joint_groups);
 // }
 
 pub(crate) enum ConstraintDesc {
@@ -45,45 +42,52 @@ pub(crate) enum ConstraintDesc {
     GroundGrouped([usize; SIMD_WIDTH]),
 }
 
-pub(crate) struct ParallelSolverConstraints<VelocityConstraint, PositionConstraint> {
+pub(crate) struct ParallelSolverConstraints<VelocityConstraint, GenVelocityConstraint> {
+    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>,
-    pub position_constraints: Vec<PositionConstraint>,
+    pub generic_velocity_constraints: Vec<GenVelocityConstraint>,
     pub constraint_descs: Vec<(usize, ConstraintDesc)>,
     pub parallel_desc_groups: Vec<usize>,
 }
 
-impl<VelocityConstraint, PositionConstraint>
-    ParallelSolverConstraints<VelocityConstraint, PositionConstraint>
+impl<VelocityConstraint, GenVelocityConstraint>
+    ParallelSolverConstraints<VelocityConstraint, GenVelocityConstraint>
 {
     pub fn new() -> Self {
         Self {
-            not_ground_interactions: Vec::new(),
-            ground_interactions: Vec::new(),
+            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::new(),
-            position_constraints: Vec::new(),
-            constraint_descs: Vec::new(),
-            parallel_desc_groups: Vec::new(),
+            velocity_constraints: vec![],
+            generic_velocity_constraints: vec![],
+            constraint_descs: vec![],
+            parallel_desc_groups: vec![],
         }
     }
 }
 
 macro_rules! impl_init_constraints_group {
-    ($VelocityConstraint: ty, $PositionConstraint: ty, $Interaction: ty,
+    ($VelocityConstraint: ty, $GenVelocityConstraint: ty, $Interaction: ty,
      $categorize: ident, $group: ident,
      $data: ident$(.$constraint_index: ident)*,
-     $num_active_constraints: path, $empty_velocity_constraint: expr, $empty_position_constraint: expr $(, $weight: ident)*) => {
-        impl ParallelSolverConstraints<$VelocityConstraint, $PositionConstraint> {
+     $num_active_constraints: path, $empty_velocity_constraint: expr $(, $weight: ident)*) => {
+        impl ParallelSolverConstraints<$VelocityConstraint, $GenVelocityConstraint> {
             pub fn init_constraint_groups<Bodies>(
                 &mut self,
                 island_id: usize,
                 islands: &IslandManager,
                 bodies: &Bodies,
+                multibody_joints: &MultibodyJointSet,
                 interactions: &mut [$Interaction],
                 interaction_groups: &ParallelInteractionGroups,
             ) where Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds> {
@@ -103,10 +107,13 @@ macro_rules! impl_init_constraints_group {
                     self.ground_interactions.clear();
                     $categorize(
                         bodies,
+                        multibody_joints,
                         interactions,
                         group,
                         &mut self.ground_interactions,
                         &mut self.not_ground_interactions,
+                        &mut self.generic_ground_interactions,
+                        &mut self.generic_not_ground_interactions,
                     );
 
                     #[cfg(feature = "simd-is-enabled")]
@@ -192,9 +199,6 @@ macro_rules! impl_init_constraints_group {
                 self.velocity_constraints.clear();
                 self.velocity_constraints
                     .resize_with(total_num_constraints, || $empty_velocity_constraint);
-                self.position_constraints.clear();
-                self.position_constraints
-                    .resize_with(total_num_constraints, || $empty_position_constraint);
             }
         }
     }
@@ -202,30 +206,28 @@ macro_rules! impl_init_constraints_group {
 
 impl_init_constraints_group!(
     AnyVelocityConstraint,
-    AnyPositionConstraint,
+    GenericVelocityConstraint,
     &mut ContactManifold,
     categorize_contacts,
     group_manifolds,
     data.constraint_index,
     VelocityConstraint::num_active_constraints,
-    AnyVelocityConstraint::Empty,
-    AnyPositionConstraint::Empty
+    AnyVelocityConstraint::Empty
 );
 
 impl_init_constraints_group!(
     AnyJointVelocityConstraint,
-    AnyJointPositionConstraint,
+    (),
     JointGraphEdge,
     categorize_joints,
     group_joints,
     constraint_index,
     AnyJointVelocityConstraint::num_active_constraints,
     AnyJointVelocityConstraint::Empty,
-    AnyJointPositionConstraint::Empty,
     weight
 );
 
-impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
+impl ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
     pub fn fill_constraints<Bodies>(
         &mut self,
         thread: &ThreadContext,
@@ -247,24 +249,20 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                     ConstraintDesc::NongroundNongrouped(manifold_id) => {
                         let manifold = &*manifolds_all[*manifold_id];
                         VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false);
-                        PositionConstraint::generate(params, manifold, bodies, &mut self.position_constraints, false);
                     }
                     ConstraintDesc::GroundNongrouped(manifold_id) => {
                         let manifold = &*manifolds_all[*manifold_id];
                         VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false);
-                        PositionGroundConstraint::generate(params, manifold, bodies, &mut self.position_constraints, false);
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(manifold_id) => {
                         let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
                         WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
-                        WPositionConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::GroundGrouped(manifold_id) => {
                         let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
                         WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
-                        WPositionGroundConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
                     }
                 }
             }
@@ -272,7 +270,7 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
     }
 }
 
-impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
+impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
     pub fn fill_constraints<Bodies>(
         &mut self,
         thread: &ThreadContext,
@@ -286,6 +284,9 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
             + ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyType>,
     {
+        return;
+
+        /*
         let descs = &self.constraint_descs;
 
         crate::concurrent_loop! {
@@ -295,35 +296,29 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
                     ConstraintDesc::NongroundNongrouped(joint_id) => {
                         let joint = &joints_all[*joint_id].weight;
                         let velocity_constraint = AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies);
-                        let position_constraint = AnyJointPositionConstraint::from_joint(joint, bodies);
                         self.velocity_constraints[joint.constraint_index] = velocity_constraint;
-                        self.position_constraints[joint.constraint_index] = position_constraint;
                     }
                     ConstraintDesc::GroundNongrouped(joint_id) => {
                         let joint = &joints_all[*joint_id].weight;
                         let velocity_constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
-                        let position_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
                         self.velocity_constraints[joint.constraint_index] = velocity_constraint;
-                        self.position_constraints[joint.constraint_index] = position_constraint;
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(joint_id) => {
-                        let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
-                        let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
-                        let position_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
-                        self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
-                        self.position_constraints[joints[0].constraint_index] = position_constraint;
+                        let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
+                        let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies);
+                        self.velocity_constraints[impulse_joints[0].constraint_index] = velocity_constraint;
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::GroundGrouped(joint_id) => {
-                        let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
-                        let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
-                        let position_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
-                        self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
-                        self.position_constraints[joints[0].constraint_index] = position_constraint;
+                        let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
+                        let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies);
+                        self.velocity_constraints[impulse_joints[0].constraint_index] = velocity_constraint;
                     }
                 }
             }
         }
+
+         */
     }
 }

src/dynamics/solver/parallel_velocity_solver.rs

@@ -1,29 +1,36 @@
 use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
-use crate::dynamics::solver::{
-    AnyJointPositionConstraint, AnyPositionConstraint, ParallelSolverConstraints,
-};
+use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint;
+use crate::dynamics::solver::ParallelSolverConstraints;
 use crate::dynamics::{IntegrationParameters, JointGraphEdge};
 use crate::geometry::ContactManifold;
 use crate::math::Real;
+use na::DVector;
 use std::sync::atomic::Ordering;
 
-pub(crate) struct ParallelVelocitySolver {}
+pub(crate) struct ParallelVelocitySolver {
+    pub mj_lambdas: Vec<DeltaVel<Real>>,
+    pub generic_mj_lambdas: DVector<Real>,
+}
 
 impl ParallelVelocitySolver {
+    pub fn new() -> Self {
+        Self {
+            mj_lambdas: Vec::new(),
+            generic_mj_lambdas: DVector::zeros(0),
+        }
+    }
+
     pub fn solve(
+        &mut self,
         thread: &ThreadContext,
         params: &IntegrationParameters,
         manifolds_all: &mut [&mut ContactManifold],
         joints_all: &mut [JointGraphEdge],
-        mj_lambdas: &mut [DeltaVel<Real>],
         contact_constraints: &mut ParallelSolverConstraints<
             AnyVelocityConstraint,
-            AnyPositionConstraint,
-        >,
-        joint_constraints: &mut ParallelSolverConstraints<
-            AnyJointVelocityConstraint,
-            AnyJointPositionConstraint,
+            GenericVelocityConstraint,
         >,
+        joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, ()>,
     ) {
         if contact_constraints.constraint_descs.is_empty()
             && joint_constraints.constraint_descs.is_empty()
@@ -31,69 +38,6 @@ impl ParallelVelocitySolver {
             return;
         }
 
-        /*
-         * Warmstart constraints.
-         */
-        {
-            // Each thread will concurrently grab thread.batch_size constraint desc to
-            // solve. If the batch size is large enough for to cross the boundary of
-            // a parallel_desc_group, we have to wait util the current group is finished
-            // before starting the next one.
-            let mut target_num_desc = 0;
-            let mut start_index = thread
-                .warmstart_constraint_index
-                .fetch_add(thread.batch_size, Ordering::SeqCst);
-            let mut batch_size = thread.batch_size;
-            let mut shift = 0;
-
-            macro_rules! warmstart(
-                ($part: expr) => {
-                    for group in $part.parallel_desc_groups.windows(2) {
-                        let num_descs_in_group = group[1] - group[0];
-                        target_num_desc += num_descs_in_group;
-
-                        while start_index < group[1] {
-                            let end_index = (start_index + batch_size).min(group[1]);
-
-                            let constraints = if end_index == $part.constraint_descs.len() {
-                                &mut $part.velocity_constraints[$part.constraint_descs[start_index].0..]
-                            } else {
-                                &mut $part.velocity_constraints[$part.constraint_descs[start_index].0..$part.constraint_descs[end_index].0]
-                            };
-
-                            for constraint in constraints {
-                                constraint.warmstart(mj_lambdas);
-                            }
-
-                            let num_solved = end_index - start_index;
-                            batch_size -= num_solved;
-
-                            thread
-                                .num_warmstarted_constraints
-                                .fetch_add(num_solved, Ordering::SeqCst);
-
-                            if batch_size == 0 {
-                                start_index = thread
-                                    .warmstart_constraint_index
-                                    .fetch_add(thread.batch_size, Ordering::SeqCst);
-                                start_index -= shift;
-                                batch_size = thread.batch_size;
-                            } else {
-                                start_index += num_solved;
-                            }
-                        }
-
-                        ThreadContext::lock_until_ge(&thread.num_warmstarted_constraints, target_num_desc);
-                    }
-                }
-            );
-
-            warmstart!(joint_constraints);
-            shift = joint_constraints.constraint_descs.len();
-            start_index -= shift;
-            warmstart!(contact_constraints);
-        }
-
         /*
          * Solve constraints.
          */
@@ -113,8 +57,8 @@ impl ParallelVelocitySolver {
 
             for _ in 0..params.max_velocity_iterations {
                 macro_rules! solve {
-                    ($part: expr) => {
-                        // Joint groups.
+                    ($part: expr, $($solve_args: expr),*) => {
+                        // ImpulseJoint groups.
                         for group in $part.parallel_desc_groups.windows(2) {
                             let num_descs_in_group = group[1] - group[0];
 
@@ -133,12 +77,10 @@ impl ParallelVelocitySolver {
                                         ..$part.constraint_descs[end_index].0]
                                 };
 
-                                //                                println!(
-                                //                                    "Solving a constraint {:?}.",
-                                //                                    rayon::current_thread_index()
-                                //                                );
                                 for constraint in constraints {
-                                    constraint.solve(mj_lambdas);
+                                    constraint.solve(
+                                        $($solve_args),*
+                                    );
                                 }
 
                                 let num_solved = end_index - start_index;
@@ -166,10 +108,15 @@ impl ParallelVelocitySolver {
                     };
                 }
 
-                solve!(joint_constraints);
+                solve!(
+                    joint_constraints,
+                    &joint_constraints.generic_jacobians,
+                    &mut self.mj_lambdas,
+                    &mut self.generic_mj_lambdas
+                );
                 shift += joint_descs.len();
                 start_index -= joint_descs.len();
-                solve!(contact_constraints);
+                solve!(contact_constraints, &mut self.mj_lambdas, true, true);
                 shift += contact_descs.len();
                 start_index -= contact_descs.len();
             }

src/dynamics/solver/position_constraint.rs

@@ -1,168 +0,0 @@
-use crate::data::ComponentSet;
-use crate::dynamics::solver::PositionGroundConstraint;
-#[cfg(feature = "simd-is-enabled")]
-use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint};
-use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
-use crate::geometry::ContactManifold;
-use crate::math::{
-    AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
-};
-use crate::utils::{WAngularInertia, WCross, WDot};
-
-pub(crate) enum AnyPositionConstraint {
-    #[cfg(feature = "simd-is-enabled")]
-    GroupedGround(WPositionGroundConstraint),
-    NonGroupedGround(PositionGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    GroupedNonGround(WPositionConstraint),
-    NonGroupedNonGround(PositionConstraint),
-    #[allow(dead_code)] // The Empty variant is only used with parallel code.
-    Empty,
-}
-
-impl AnyPositionConstraint {
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        match self {
-            #[cfg(feature = "simd-is-enabled")]
-            AnyPositionConstraint::GroupedGround(c) => c.solve(params, positions),
-            AnyPositionConstraint::NonGroupedGround(c) => c.solve(params, positions),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyPositionConstraint::GroupedNonGround(c) => c.solve(params, positions),
-            AnyPositionConstraint::NonGroupedNonGround(c) => c.solve(params, positions),
-            AnyPositionConstraint::Empty => unreachable!(),
-        }
-    }
-}
-
-pub(crate) struct PositionConstraint {
-    pub rb1: usize,
-    pub rb2: usize,
-    // NOTE: the points are relative to the center of masses.
-    pub local_p1: [Point<Real>; MAX_MANIFOLD_POINTS],
-    pub local_p2: [Point<Real>; MAX_MANIFOLD_POINTS],
-    pub dists: [Real; MAX_MANIFOLD_POINTS],
-    pub local_n1: Vector<Real>,
-    pub num_contacts: u8,
-    pub im1: Real,
-    pub im2: Real,
-    pub ii1: AngularInertia<Real>,
-    pub ii2: AngularInertia<Real>,
-    pub erp: Real,
-    pub max_linear_correction: Real,
-}
-
-impl PositionConstraint {
-    pub fn generate<Bodies>(
-        params: &IntegrationParameters,
-        manifold: &ContactManifold,
-        bodies: &Bodies,
-        out_constraints: &mut Vec<AnyPositionConstraint>,
-        push: bool,
-    ) where
-        Bodies: ComponentSet<RigidBodyPosition>
-            + ComponentSet<RigidBodyMassProps>
-            + ComponentSet<RigidBodyIds>,
-    {
-        let handle1 = manifold.data.rigid_body1.unwrap();
-        let handle2 = manifold.data.rigid_body2.unwrap();
-
-        let ids1: &RigidBodyIds = bodies.index(handle1.0);
-        let ids2: &RigidBodyIds = bodies.index(handle2.0);
-        let poss1: &RigidBodyPosition = bodies.index(handle1.0);
-        let poss2: &RigidBodyPosition = bodies.index(handle2.0);
-        let mprops1: &RigidBodyMassProps = bodies.index(handle1.0);
-        let mprops2: &RigidBodyMassProps = bodies.index(handle2.0);
-
-        for (l, manifold_points) in manifold
-            .data
-            .solver_contacts
-            .chunks(MAX_MANIFOLD_POINTS)
-            .enumerate()
-        {
-            let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
-            let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
-            let mut dists = [0.0; MAX_MANIFOLD_POINTS];
-
-            for l in 0..manifold_points.len() {
-                local_p1[l] = poss1
-                    .position
-                    .inverse_transform_point(&manifold_points[l].point);
-                local_p2[l] = poss2
-                    .position
-                    .inverse_transform_point(&manifold_points[l].point);
-                dists[l] = manifold_points[l].dist;
-            }
-
-            let constraint = PositionConstraint {
-                rb1: ids1.active_set_offset,
-                rb2: ids2.active_set_offset,
-                local_p1,
-                local_p2,
-                local_n1: poss1
-                    .position
-                    .inverse_transform_vector(&manifold.data.normal),
-                dists,
-                im1: mprops1.effective_inv_mass,
-                im2: mprops2.effective_inv_mass,
-                ii1: mprops1.effective_world_inv_inertia_sqrt.squared(),
-                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
-                num_contacts: manifold_points.len() as u8,
-                erp: params.erp,
-                max_linear_correction: params.max_linear_correction,
-            };
-
-            if push {
-                out_constraints.push(AnyPositionConstraint::NonGroupedNonGround(constraint));
-            } else {
-                out_constraints[manifold.data.constraint_index + l] =
-                    AnyPositionConstraint::NonGroupedNonGround(constraint);
-            }
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        // FIXME: can we avoid most of the multiplications by pos1/pos2?
-        // Compute jacobians.
-        let mut pos1 = positions[self.rb1];
-        let mut pos2 = positions[self.rb2];
-        let allowed_err = params.allowed_linear_error;
-
-        for k in 0..self.num_contacts as usize {
-            let target_dist = -self.dists[k] - allowed_err;
-            let n1 = pos1 * self.local_n1;
-            let p1 = pos1 * self.local_p1[k];
-            let p2 = pos2 * self.local_p2[k];
-            let dpos = p2 - p1;
-            let dist = dpos.dot(&n1);
-
-            if dist < target_dist {
-                let p1 = p2 - n1 * dist;
-                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
-                let dp1 = p1.coords - pos1.translation.vector;
-                let dp2 = p2.coords - pos2.translation.vector;
-
-                let gcross1 = dp1.gcross(n1);
-                let gcross2 = -dp2.gcross(n1);
-                let ii_gcross1 = self.ii1.transform_vector(gcross1);
-                let ii_gcross2 = self.ii2.transform_vector(gcross2);
-
-                // Compute impulse.
-                let inv_r =
-                    self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
-                let impulse = err / inv_r;
-
-                // Apply impulse.
-                let tra1 = Translation::from(n1 * (impulse * self.im1));
-                let tra2 = Translation::from(n1 * (-impulse * self.im2));
-                let rot1 = Rotation::new(ii_gcross1 * impulse);
-                let rot2 = Rotation::new(ii_gcross2 * impulse);
-
-                pos1 = Isometry::from_parts(tra1 * pos1.translation, rot1 * pos1.rotation);
-                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
-            }
-        }
-
-        positions[self.rb1] = pos1;
-        positions[self.rb2] = pos2;
-    }
-}

src/dynamics/solver/position_constraint_wide.rs

@@ -1,157 +0,0 @@
-use super::AnyPositionConstraint;
-use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
-use crate::geometry::ContactManifold;
-use crate::math::{
-    AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
-    MAX_MANIFOLD_POINTS, SIMD_WIDTH,
-};
-use crate::utils::{WAngularInertia, WCross, WDot};
-
-use crate::data::ComponentSet;
-use num::Zero;
-use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
-
-pub(crate) struct WPositionConstraint {
-    pub rb1: [usize; SIMD_WIDTH],
-    pub rb2: [usize; SIMD_WIDTH],
-    // NOTE: the points are relative to the center of masses.
-    pub local_p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
-    pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
-    pub dists: [SimdReal; MAX_MANIFOLD_POINTS],
-    pub local_n1: Vector<SimdReal>,
-    pub im1: SimdReal,
-    pub im2: SimdReal,
-    pub ii1: AngularInertia<SimdReal>,
-    pub ii2: AngularInertia<SimdReal>,
-    pub erp: SimdReal,
-    pub max_linear_correction: SimdReal,
-    pub num_contacts: u8,
-}
-
-impl WPositionConstraint {
-    pub fn generate<Bodies>(
-        params: &IntegrationParameters,
-        manifolds: [&ContactManifold; SIMD_WIDTH],
-        bodies: &Bodies,
-        out_constraints: &mut Vec<AnyPositionConstraint>,
-        push: bool,
-    ) where
-        Bodies: ComponentSet<RigidBodyPosition>
-            + ComponentSet<RigidBodyMassProps>
-            + ComponentSet<RigidBodyIds>,
-    {
-        let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
-        let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
-
-        let poss1: [&RigidBodyPosition; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
-        let poss2: [&RigidBodyPosition; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
-        let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
-        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
-        let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
-        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
-
-        let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
-        let sqrt_ii1: AngularInertia<SimdReal> =
-            AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let sqrt_ii2: AngularInertia<SimdReal> =
-            AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
-
-        let pos1 = Isometry::from(gather![|ii| poss1[ii].position]);
-        let pos2 = Isometry::from(gather![|ii| poss2[ii].position]);
-
-        let local_n1 =
-            pos1.inverse_transform_vector(&Vector::from(gather![|ii| manifolds[ii].data.normal]));
-
-        let rb1 = gather![|ii| ids1[ii].active_set_offset];
-        let rb2 = gather![|ii| ids2[ii].active_set_offset];
-
-        let num_active_contacts = manifolds[0].data.num_active_contacts();
-
-        for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
-            let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
-
-            let mut constraint = WPositionConstraint {
-                rb1,
-                rb2,
-                local_p1: [Point::origin(); MAX_MANIFOLD_POINTS],
-                local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
-                local_n1,
-                dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
-                im1,
-                im2,
-                ii1: sqrt_ii1.squared(),
-                ii2: sqrt_ii2.squared(),
-                erp: SimdReal::splat(params.erp),
-                max_linear_correction: SimdReal::splat(params.max_linear_correction),
-                num_contacts: num_points as u8,
-            };
-
-            for i in 0..num_points {
-                let point = Point::from(gather![|ii| manifold_points[ii][i].point]);
-                let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]);
-                constraint.local_p1[i] = pos1.inverse_transform_point(&point);
-                constraint.local_p2[i] = pos2.inverse_transform_point(&point);
-                constraint.dists[i] = dist;
-            }
-
-            if push {
-                out_constraints.push(AnyPositionConstraint::GroupedNonGround(constraint));
-            } else {
-                out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
-                    AnyPositionConstraint::GroupedNonGround(constraint);
-            }
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        // FIXME: can we avoid most of the multiplications by pos1/pos2?
-        // Compute jacobians.
-        let mut pos1 = Isometry::from(gather![|ii| positions[self.rb1[ii]]]);
-        let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]);
-        let allowed_err = SimdReal::splat(params.allowed_linear_error);
-
-        for k in 0..self.num_contacts as usize {
-            let target_dist = -self.dists[k] - allowed_err;
-            let n1 = pos1 * self.local_n1;
-            let p1 = pos1 * self.local_p1[k];
-            let p2 = pos2 * self.local_p2[k];
-            let dpos = p2 - p1;
-            let dist = dpos.dot(&n1);
-
-            // NOTE: this condition does not seem to be useful perfomancewise?
-            if dist.simd_lt(target_dist).any() {
-                // NOTE: only works for the point-point case.
-                let p1 = p2 - n1 * dist;
-                let err = ((dist - target_dist) * self.erp)
-                    .simd_clamp(-self.max_linear_correction, SimdReal::zero());
-                let dp1 = p1.coords - pos1.translation.vector;
-                let dp2 = p2.coords - pos2.translation.vector;
-
-                let gcross1 = dp1.gcross(n1);
-                let gcross2 = -dp2.gcross(n1);
-                let ii_gcross1 = self.ii1.transform_vector(gcross1);
-                let ii_gcross2 = self.ii2.transform_vector(gcross2);
-
-                // Compute impulse.
-                let inv_r =
-                    self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
-                let impulse = err / inv_r;
-
-                // Apply impulse.
-                pos1.translation = Translation::from(n1 * (impulse * self.im1)) * pos1.translation;
-                pos1.rotation = Rotation::new(ii_gcross1 * impulse) * pos1.rotation;
-                pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation;
-                pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
-            }
-        }
-
-        for ii in 0..SIMD_WIDTH {
-            positions[self.rb1[ii]] = pos1.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            positions[self.rb2[ii]] = pos2.extract(ii);
-        }
-    }
-}

src/dynamics/solver/position_ground_constraint.rs

@@ -1,121 +0,0 @@
-use super::AnyPositionConstraint;
-use crate::data::{BundleSet, ComponentSet};
-use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
-use crate::geometry::ContactManifold;
-use crate::math::{
-    AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
-};
-use crate::utils::{WAngularInertia, WCross, WDot};
-
-pub(crate) struct PositionGroundConstraint {
-    pub rb2: usize,
-    // NOTE: the points are relative to the center of masses.
-    pub p1: [Point<Real>; MAX_MANIFOLD_POINTS],
-    pub local_p2: [Point<Real>; MAX_MANIFOLD_POINTS],
-    pub dists: [Real; MAX_MANIFOLD_POINTS],
-    pub n1: Vector<Real>,
-    pub num_contacts: u8,
-    pub im2: Real,
-    pub ii2: AngularInertia<Real>,
-    pub erp: Real,
-    pub max_linear_correction: Real,
-}
-
-impl PositionGroundConstraint {
-    pub fn generate<Bodies>(
-        params: &IntegrationParameters,
-        manifold: &ContactManifold,
-        bodies: &Bodies,
-        out_constraints: &mut Vec<AnyPositionConstraint>,
-        push: bool,
-    ) where
-        Bodies: ComponentSet<RigidBodyPosition>
-            + ComponentSet<RigidBodyMassProps>
-            + ComponentSet<RigidBodyIds>,
-    {
-        let flip = manifold.data.relative_dominance < 0;
-
-        let (handle2, n1) = if flip {
-            (manifold.data.rigid_body1.unwrap(), -manifold.data.normal)
-        } else {
-            (manifold.data.rigid_body2.unwrap(), manifold.data.normal)
-        };
-
-        let (ids2, poss2, mprops2): (&RigidBodyIds, &RigidBodyPosition, &RigidBodyMassProps) =
-            bodies.index_bundle(handle2.0);
-
-        for (l, manifold_contacts) in manifold
-            .data
-            .solver_contacts
-            .chunks(MAX_MANIFOLD_POINTS)
-            .enumerate()
-        {
-            let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
-            let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
-            let mut dists = [0.0; MAX_MANIFOLD_POINTS];
-
-            for k in 0..manifold_contacts.len() {
-                p1[k] = manifold_contacts[k].point;
-                local_p2[k] = poss2
-                    .position
-                    .inverse_transform_point(&manifold_contacts[k].point);
-                dists[k] = manifold_contacts[k].dist;
-            }
-
-            let constraint = PositionGroundConstraint {
-                rb2: ids2.active_set_offset,
-                p1,
-                local_p2,
-                n1,
-                dists,
-                im2: mprops2.effective_inv_mass,
-                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
-                num_contacts: manifold_contacts.len() as u8,
-                erp: params.erp,
-                max_linear_correction: params.max_linear_correction,
-            };
-
-            if push {
-                out_constraints.push(AnyPositionConstraint::NonGroupedGround(constraint));
-            } else {
-                out_constraints[manifold.data.constraint_index + l] =
-                    AnyPositionConstraint::NonGroupedGround(constraint);
-            }
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        // FIXME: can we avoid most of the multiplications by pos1/pos2?
-        // Compute jacobians.
-        let mut pos2 = positions[self.rb2];
-        let allowed_err = params.allowed_linear_error;
-
-        for k in 0..self.num_contacts as usize {
-            let target_dist = -self.dists[k] - allowed_err;
-            let n1 = self.n1;
-            let p1 = self.p1[k];
-            let p2 = pos2 * self.local_p2[k];
-            let dpos = p2 - p1;
-            let dist = dpos.dot(&n1);
-
-            if dist < target_dist {
-                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
-                let dp2 = p2.coords - pos2.translation.vector;
-
-                let gcross2 = -dp2.gcross(n1);
-                let ii_gcross2 = self.ii2.transform_vector(gcross2);
-
-                // Compute impulse.
-                let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
-                let impulse = err / inv_r;
-
-                // Apply impulse.
-                let tra2 = Translation::from(n1 * (-impulse * self.im2));
-                let rot2 = Rotation::new(ii_gcross2 * impulse);
-                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
-            }
-        }
-
-        positions[self.rb2] = pos2;
-    }
-}

src/dynamics/solver/position_ground_constraint_wide.rs

@@ -1,143 +0,0 @@
-use super::AnyPositionConstraint;
-use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
-use crate::geometry::ContactManifold;
-use crate::math::{
-    AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
-    MAX_MANIFOLD_POINTS, SIMD_WIDTH,
-};
-use crate::utils::{WAngularInertia, WCross, WDot};
-
-use crate::data::ComponentSet;
-use num::Zero;
-use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
-
-pub(crate) struct WPositionGroundConstraint {
-    pub rb2: [usize; SIMD_WIDTH],
-    // NOTE: the points are relative to the center of masses.
-    pub p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
-    pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
-    pub dists: [SimdReal; MAX_MANIFOLD_POINTS],
-    pub n1: Vector<SimdReal>,
-    pub im2: SimdReal,
-    pub ii2: AngularInertia<SimdReal>,
-    pub erp: SimdReal,
-    pub max_linear_correction: SimdReal,
-    pub num_contacts: u8,
-}
-
-impl WPositionGroundConstraint {
-    pub fn generate<Bodies>(
-        params: &IntegrationParameters,
-        manifolds: [&ContactManifold; SIMD_WIDTH],
-        bodies: &Bodies,
-        out_constraints: &mut Vec<AnyPositionConstraint>,
-        push: bool,
-    ) where
-        Bodies: ComponentSet<RigidBodyIds>
-            + ComponentSet<RigidBodyPosition>
-            + ComponentSet<RigidBodyMassProps>,
-    {
-        let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
-        let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
-        let mut flipped = [false; SIMD_WIDTH];
-
-        for ii in 0..SIMD_WIDTH {
-            if manifolds[ii].data.relative_dominance < 0 {
-                flipped[ii] = true;
-                std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
-            }
-        }
-
-        let poss2: [&RigidBodyPosition; SIMD_WIDTH] =
-            gather![|ii| bodies.index(handles2[ii].unwrap().0)];
-        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)];
-        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
-            gather![|ii| bodies.index(handles2[ii].unwrap().0)];
-
-        let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
-        let sqrt_ii2: AngularInertia<SimdReal> =
-            AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
-
-        let n1 = Vector::from(gather![|ii| if flipped[ii] {
-            -manifolds[ii].data.normal
-        } else {
-            manifolds[ii].data.normal
-        }]);
-
-        let pos2 = Isometry::from(gather![|ii| poss2[ii].position]);
-        let rb2 = gather![|ii| ids2[ii].active_set_offset];
-
-        let num_active_contacts = manifolds[0].data.num_active_contacts();
-
-        for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
-            let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
-
-            let mut constraint = WPositionGroundConstraint {
-                rb2,
-                p1: [Point::origin(); MAX_MANIFOLD_POINTS],
-                local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
-                n1,
-                dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
-                im2,
-                ii2: sqrt_ii2.squared(),
-                erp: SimdReal::splat(params.erp),
-                max_linear_correction: SimdReal::splat(params.max_linear_correction),
-                num_contacts: num_points as u8,
-            };
-
-            for i in 0..num_points {
-                let point = Point::from(gather![|ii| manifold_points[ii][i].point]);
-                let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]);
-                constraint.p1[i] = point;
-                constraint.local_p2[i] = pos2.inverse_transform_point(&point);
-                constraint.dists[i] = dist;
-            }
-
-            if push {
-                out_constraints.push(AnyPositionConstraint::GroupedGround(constraint));
-            } else {
-                out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
-                    AnyPositionConstraint::GroupedGround(constraint);
-            }
-        }
-    }
-
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        // FIXME: can we avoid most of the multiplications by pos1/pos2?
-        // Compute jacobians.
-        let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]);
-        let allowed_err = SimdReal::splat(params.allowed_linear_error);
-
-        for k in 0..self.num_contacts as usize {
-            let target_dist = -self.dists[k] - allowed_err;
-            let n1 = self.n1;
-            let p1 = self.p1[k];
-            let p2 = pos2 * self.local_p2[k];
-            let dpos = p2 - p1;
-            let dist = dpos.dot(&n1);
-
-            // NOTE: this condition does not seem to be useful perfomancewise?
-            if dist.simd_lt(target_dist).any() {
-                let err = ((dist - target_dist) * self.erp)
-                    .simd_clamp(-self.max_linear_correction, SimdReal::zero());
-                let dp2 = p2.coords - pos2.translation.vector;
-
-                let gcross2 = -dp2.gcross(n1);
-                let ii_gcross2 = self.ii2.transform_vector(gcross2);
-
-                // Compute impulse.
-                let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
-                let impulse = err / inv_r;
-
-                // Apply impulse.
-                pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation;
-                pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
-            }
-        }
-
-        for ii in 0..SIMD_WIDTH {
-            positions[self.rb2[ii]] = pos2.extract(ii);
-        }
-    }
-}

src/dynamics/solver/position_solver.rs

@@ -1,57 +0,0 @@
-use super::AnyJointPositionConstraint;
-use crate::data::{ComponentSet, ComponentSetMut};
-use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters};
-use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyPosition};
-use crate::math::{Isometry, Real};
-
-pub(crate) struct PositionSolver {
-    positions: Vec<Isometry<Real>>,
-}
-
-impl PositionSolver {
-    pub fn new() -> Self {
-        Self {
-            positions: Vec::new(),
-        }
-    }
-
-    pub fn solve<Bodies>(
-        &mut self,
-        island_id: usize,
-        params: &IntegrationParameters,
-        islands: &IslandManager,
-        bodies: &mut Bodies,
-        contact_constraints: &[AnyPositionConstraint],
-        joint_constraints: &[AnyJointPositionConstraint],
-    ) where
-        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
-    {
-        if contact_constraints.is_empty() && joint_constraints.is_empty() {
-            // Nothing to do.
-            return;
-        }
-
-        self.positions.clear();
-        self.positions
-            .extend(islands.active_island(island_id).iter().map(|h| {
-                let poss: &RigidBodyPosition = bodies.index(h.0);
-                poss.next_position
-            }));
-
-        for _ in 0..params.max_position_iterations {
-            for constraint in joint_constraints {
-                constraint.solve(params, &mut self.positions)
-            }
-
-            for constraint in contact_constraints {
-                constraint.solve(params, &mut self.positions)
-            }
-        }
-
-        for handle in islands.active_island(island_id) {
-            let ids: &RigidBodyIds = bodies.index(handle.0);
-            let next_pos = &self.positions[ids.active_set_offset];
-            bodies.map_mut_internal(handle.0, |poss| poss.next_position = *next_pos);
-        }
-    }
-}

src/dynamics/solver/solver_constraints.rs

@@ -2,62 +2,69 @@ use super::{
     AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint,
 };
 #[cfg(feature = "simd-is-enabled")]
-use super::{
-    WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint,
-};
+use super::{WVelocityConstraint, WVelocityGroundConstraint};
 use crate::data::ComponentSet;
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
-use crate::dynamics::solver::{
-    AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint,
-};
+use crate::dynamics::solver::GenericVelocityConstraint;
 use crate::dynamics::{
-    solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
+    solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex,
+    MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
 };
 use crate::dynamics::{IslandManager, RigidBodyVelocity};
 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, PositionConstraint> {
+pub(crate) struct SolverConstraints<VelocityConstraint, GenVelocityConstraint> {
+    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>,
-    pub position_constraints: Vec<PositionConstraint>,
+    pub generic_velocity_constraints: Vec<GenVelocityConstraint>,
 }
 
-impl<VelocityConstraint, PositionConstraint>
-    SolverConstraints<VelocityConstraint, PositionConstraint>
+impl<VelocityConstraint, GenVelocityConstraint>
+    SolverConstraints<VelocityConstraint, GenVelocityConstraint>
 {
     pub fn new() -> Self {
         Self {
-            not_ground_interactions: Vec::new(),
-            ground_interactions: Vec::new(),
+            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::new(),
-            position_constraints: Vec::new(),
+            velocity_constraints: vec![],
+            generic_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();
-        self.position_constraints.clear();
+        self.generic_velocity_constraints.clear();
     }
 }
 
-impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
+impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
     pub fn init_constraint_groups<Bodies>(
         &mut self,
         island_id: usize,
         islands: &IslandManager,
         bodies: &Bodies,
+        multibody_joints: &MultibodyJointSet,
         manifolds: &[&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
     ) where
@@ -65,12 +72,18 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
     {
         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_not_ground_interactions,
+            &mut self.generic_ground_interactions,
         );
 
         self.interaction_groups.clear_groups();
@@ -106,6 +119,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         params: &IntegrationParameters,
         islands: &IslandManager,
         bodies: &Bodies,
+        multibody_joints: &MultibodyJointSet,
         manifolds: &[&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
     ) where
@@ -116,15 +130,24 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
             + ComponentSet<RigidBodyType>,
     {
         self.velocity_constraints.clear();
-        self.position_constraints.clear();
+        self.generic_velocity_constraints.clear();
 
-        self.init_constraint_groups(island_id, islands, bodies, manifolds, manifold_indices);
+        self.init_constraint_groups(
+            island_id,
+            islands,
+            bodies,
+            multibody_joints,
+            manifolds,
+            manifold_indices,
+        );
 
         #[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);
+
         #[cfg(feature = "simd-is-enabled")]
         {
             self.compute_grouped_ground_constraints(params, bodies, manifolds);
@@ -159,13 +182,6 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 &mut self.velocity_constraints,
                 true,
             );
-            WPositionConstraint::generate(
-                params,
-                manifolds,
-                bodies,
-                &mut self.position_constraints,
-                true,
-            );
         }
     }
 
@@ -190,11 +206,34 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 &mut self.velocity_constraints,
                 true,
             );
-            PositionConstraint::generate(
+        }
+    }
+
+    fn compute_generic_constraints<Bodies>(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &Bodies,
+        multibody_joints: &MultibodyJointSet,
+        manifolds_all: &[&mut ContactManifold],
+    ) where
+        Bodies: ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>,
+    {
+        let mut jacobian_id = 0;
+        for manifold_i in &self.generic_not_ground_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            GenericVelocityConstraint::generate(
                 params,
+                *manifold_i,
                 manifold,
                 bodies,
-                &mut self.position_constraints,
+                multibody_joints,
+                &mut self.generic_velocity_constraints,
+                &mut self.generic_jacobians,
+                &mut jacobian_id,
                 true,
             );
         }
@@ -227,13 +266,6 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 &mut self.velocity_constraints,
                 true,
             );
-            WPositionGroundConstraint::generate(
-                params,
-                manifolds,
-                bodies,
-                &mut self.position_constraints,
-                true,
-            );
         }
     }
 
@@ -258,25 +290,19 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 &mut self.velocity_constraints,
                 true,
             );
-            PositionGroundConstraint::generate(
-                params,
-                manifold,
-                bodies,
-                &mut self.position_constraints,
-                true,
-            )
         }
     }
 }
 
-impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
+impl SolverConstraints<AnyJointVelocityConstraint, ()> {
     pub fn init<Bodies>(
         &mut self,
         island_id: usize,
         params: &IntegrationParameters,
         islands: &IslandManager,
         bodies: &Bodies,
-        joints: &[JointGraphEdge],
+        multibody_joints: &MultibodyJointSet,
+        impulse_joints: &[JointGraphEdge],
         joint_constraint_indices: &[JointIndex],
     ) where
         Bodies: ComponentSet<RigidBodyType>
@@ -285,26 +311,32 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
             + ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyMassProps>,
     {
-        // Generate constraints for joints.
+        // 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,
-            joints,
+            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.position_constraints.clear();
+        self.generic_velocity_constraints.clear();
 
         self.interaction_groups.clear_groups();
         self.interaction_groups.group_joints(
             island_id,
             islands,
             bodies,
-            joints,
+            impulse_joints,
             &self.not_ground_interactions,
         );
 
@@ -313,7 +345,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
             island_id,
             islands,
             bodies,
-            joints,
+            impulse_joints,
             &self.ground_interactions,
         );
         // NOTE: uncomment this do disable SIMD joint resolution.
@@ -324,15 +356,72 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
         //     .nongrouped_interactions
         //     .append(&mut self.ground_interaction_groups.grouped_interactions);
 
-        self.compute_nongrouped_joint_ground_constraints(params, bodies, joints);
+        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_ground_constraints(params, bodies, joints);
+            self.compute_grouped_joint_constraints(params, bodies, impulse_joints);
         }
-        self.compute_nongrouped_joint_constraints(params, bodies, 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_constraints(params, bodies, joints);
+            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,
+                    )
+                }
+            }
         }
     }
 
@@ -340,6 +429,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
         &mut self,
         params: &IntegrationParameters,
         bodies: &Bodies,
+        multibody_joints: &MultibodyJointSet,
         joints_all: &[JointGraphEdge],
     ) where
         Bodies: ComponentSet<RigidBodyType>
@@ -348,13 +438,19 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
             + ComponentSet<RigidBodyVelocity>
             + ComponentSet<RigidBodyIds>,
     {
+        let mut j_id = 0;
         for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
             let joint = &joints_all[*joint_i].weight;
-            let vel_constraint =
-                AnyJointVelocityConstraint::from_joint_ground(params, *joint_i, joint, bodies);
-            self.velocity_constraints.push(vel_constraint);
-            let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
-            self.position_constraints.push(pos_constraint);
+            AnyJointVelocityConstraint::from_joint_ground(
+                params,
+                *joint_i,
+                joint,
+                bodies,
+                multibody_joints,
+                &mut j_id,
+                &mut self.generic_jacobians,
+                &mut self.velocity_constraints,
+            );
         }
     }
 
@@ -377,14 +473,14 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
             .chunks_exact(SIMD_WIDTH)
         {
             let joints_id = gather![|ii| joints_i[ii]];
-            let joints = gather![|ii| &joints_all[joints_i[ii]].weight];
-            let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(
-                params, joints_id, joints, bodies,
+            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,
             );
-            self.velocity_constraints.push(vel_constraint);
-
-            let pos_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
-            self.position_constraints.push(pos_constraint);
         }
     }
 
@@ -392,7 +488,9 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
         &mut self,
         params: &IntegrationParameters,
         bodies: &Bodies,
+        multibody_joints: &MultibodyJointSet,
         joints_all: &[JointGraphEdge],
+        j_id: &mut usize,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
@@ -401,11 +499,73 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
     {
         for joint_i in &self.interaction_groups.nongrouped_interactions {
             let joint = &joints_all[*joint_i].weight;
-            let vel_constraint =
-                AnyJointVelocityConstraint::from_joint(params, *joint_i, joint, bodies);
-            self.velocity_constraints.push(vel_constraint);
-            let pos_constraint = AnyJointPositionConstraint::from_joint(joint, bodies);
-            self.position_constraints.push(pos_constraint);
+            AnyJointVelocityConstraint::from_joint(
+                params,
+                *joint_i,
+                joint,
+                bodies,
+                multibody_joints,
+                j_id,
+                &mut self.generic_jacobians,
+                &mut self.velocity_constraints,
+            );
+        }
+    }
+
+    fn compute_generic_joint_constraints<Bodies>(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &Bodies,
+        multibody_joints: &MultibodyJointSet,
+        joints_all: &[JointGraphEdge],
+        j_id: &mut usize,
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
+        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,
+            )
+        }
+    }
+
+    fn compute_generic_ground_joint_constraints<Bodies>(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &Bodies,
+        multibody_joints: &MultibodyJointSet,
+        joints_all: &[JointGraphEdge],
+        j_id: &mut usize,
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyIds>,
+    {
+        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,
+            )
         }
     }
 
@@ -427,13 +587,14 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
             .chunks_exact(SIMD_WIDTH)
         {
             let joints_id = gather![|ii| joints_i[ii]];
-            let joints = gather![|ii| &joints_all[joints_i[ii]].weight];
-            let vel_constraint =
-                AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
-            self.velocity_constraints.push(vel_constraint);
-
-            let pos_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
-            self.position_constraints.push(pos_constraint);
+            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,
+            );
         }
     }
 }

src/dynamics/solver/velocity_constraint.rs

@@ -41,26 +41,37 @@ impl AnyVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    pub fn remove_bias_from_rhs(&mut self) {
         match self {
-            AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas),
-            AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas),
+            AnyVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(),
+            AnyVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(),
             #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::GroupedGround(c) => c.warmstart(mj_lambdas),
+            AnyVelocityConstraint::Grouped(c) => c.remove_bias_from_rhs(),
             #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::Grouped(c) => c.warmstart(mj_lambdas),
-            AnyVelocityConstraint::Empty => unreachable!(),
+            AnyVelocityConstraint::GroupedGround(c) => c.remove_bias_from_rhs(),
+            AnyVelocityConstraint::Empty => {}
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    pub fn solve(
+        &mut self,
+        mj_lambdas: &mut [DeltaVel<Real>],
+        solve_normal: bool,
+        solve_friction: bool,
+    ) {
         match self {
-            AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas),
-            AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas),
+            AnyVelocityConstraint::NongroupedGround(c) => {
+                c.solve(mj_lambdas, solve_normal, solve_friction)
+            }
+            AnyVelocityConstraint::Nongrouped(c) => {
+                c.solve(mj_lambdas, solve_normal, solve_friction)
+            }
             #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::GroupedGround(c) => c.solve(mj_lambdas),
+            AnyVelocityConstraint::GroupedGround(c) => {
+                c.solve(mj_lambdas, solve_normal, solve_friction)
+            }
             #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas),
+            AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas, solve_normal, solve_friction),
             AnyVelocityConstraint::Empty => unreachable!(),
         }
     }
@@ -83,8 +94,6 @@ 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.
-    #[cfg(feature = "dim3")]
-    pub tangent_rot1: na::UnitComplex<Real>, // Orientation of the tangent basis wrt. the reference basis.
     pub im1: Real,
     pub im2: Real,
     pub limit: Real,
@@ -118,7 +127,7 @@ impl VelocityConstraint {
         assert_eq!(manifold.data.relative_dominance, 0);
 
         let inv_dt = params.inv_dt();
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        let erp_inv_dt = params.erp_inv_dt();
 
         let handle1 = manifold.data.rigid_body1.unwrap();
         let handle2 = manifold.data.rigid_body2.unwrap();
@@ -130,12 +139,11 @@ impl VelocityConstraint {
         let mj_lambda1 = ids1.active_set_offset;
         let mj_lambda2 = ids2.active_set_offset;
         let force_dir1 = -manifold.data.normal;
-        let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
 
         #[cfg(feature = "dim2")]
         let tangents1 = force_dir1.orthonormal_basis();
         #[cfg(feature = "dim3")]
-        let (tangents1, tangent_rot1) =
+        let tangents1 =
             super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
 
         for (_l, manifold_points) in manifold
@@ -149,8 +157,6 @@ impl VelocityConstraint {
                 dir1: force_dir1,
                 #[cfg(feature = "dim3")]
                 tangent1: tangents1[0],
-                #[cfg(feature = "dim3")]
-                tangent_rot1,
                 elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im1: mprops1.effective_inv_mass,
                 im2: mprops2.effective_inv_mass,
@@ -171,7 +177,7 @@ impl VelocityConstraint {
             // avoid spurious copying.
             // Is this optimization beneficial when targeting non-WASM platforms?
             //
-            // NOTE: joints have the same problem, but it is not easy to refactor the code that way
+            // 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 push {
@@ -198,7 +204,6 @@ impl VelocityConstraint {
                 #[cfg(feature = "dim3")]
                 {
                     constraint.tangent1 = tangents1[0];
-                    constraint.tangent_rot1 = tangent_rot1;
                 }
                 constraint.im1 = mprops1.effective_inv_mass;
                 constraint.im2 = mprops2.effective_inv_mass;
@@ -218,8 +223,6 @@ impl VelocityConstraint {
                 let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
                 let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
 
-                let warmstart_correction;
-
                 constraint.limit = manifold_point.friction;
                 constraint.manifold_contact_id[k] = manifold_point.contact_id;
 
@@ -241,34 +244,28 @@ impl VelocityConstraint {
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
                     let is_resting = 1.0 - is_bouncy;
 
-                    let mut rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
-                    rhs += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs *= is_bouncy + is_resting * params.velocity_solve_fraction;
-                    rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0);
-                    warmstart_correction = (params.warmstart_correction_slope
-                        / (rhs - manifold_point.prev_rhs).abs())
-                    .min(warmstart_coeff);
+                    rhs_wo_bias +=
+                        (manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt;
+                    rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    let rhs_bias = /* is_resting
+                        * */  erp_inv_dt
+                        * (manifold_point.dist + params.allowed_linear_error).min(0.0);
 
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
-                        rhs,
-                        impulse: manifold_point.warmstart_impulse * warmstart_correction,
+                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs_wo_bias,
+                        impulse: 0.0,
                         r,
                     };
                 }
 
                 // Tangent parts.
                 {
-                    #[cfg(feature = "dim3")]
-                    let impulse = tangent_rot1
-                        * manifold_points[k].warmstart_tangent_impulse
-                        * warmstart_correction;
-                    #[cfg(feature = "dim2")]
-                    let impulse =
-                        [manifold_points[k].warmstart_tangent_impulse * warmstart_correction];
-                    constraint.elements[k].tangent_part.impulse = impulse;
+                    constraint.elements[k].tangent_part.impulse = na::zero();
 
                     for j in 0..DIM - 1 {
                         let gcross1 = mprops1
@@ -303,26 +300,12 @@ impl VelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda1 = DeltaVel::zero();
-        let mut mj_lambda2 = DeltaVel::zero();
-
-        VelocityConstraintElement::warmstart_group(
-            &self.elements[..self.num_contacts as usize],
-            &self.dir1,
-            #[cfg(feature = "dim3")]
-            &self.tangent1,
-            self.im1,
-            self.im2,
-            &mut mj_lambda1,
-            &mut mj_lambda2,
-        );
-
-        mj_lambdas[self.mj_lambda1 as usize] += mj_lambda1;
-        mj_lambdas[self.mj_lambda2 as usize] += mj_lambda2;
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    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];
 
@@ -336,6 +319,8 @@ impl VelocityConstraint {
             self.limit,
             &mut mj_lambda1,
             &mut mj_lambda2,
+            solve_normal,
+            solve_friction,
         );
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
@@ -349,7 +334,6 @@ impl VelocityConstraint {
             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;
-            active_contact.data.rhs = self.elements[k].normal_part.rhs;
 
             #[cfg(feature = "dim2")]
             {
@@ -357,12 +341,16 @@ impl VelocityConstraint {
             }
             #[cfg(feature = "dim3")]
             {
-                active_contact.data.tangent_impulse = self
-                    .tangent_rot1
-                    .inverse_transform_vector(&self.elements[k].tangent_part.impulse);
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
             }
         }
     }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        for elt in &mut self.elements {
+            elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
+        }
+    }
 }
 
 #[inline(always)]
@@ -371,7 +359,7 @@ pub(crate) fn compute_tangent_contact_directions<N>(
     force_dir1: &Vector<N>,
     linvel1: &Vector<N>,
     linvel2: &Vector<N>,
-) -> ([Vector<N>; DIM - 1], na::UnitComplex<N>)
+) -> [Vector<N>; DIM - 1]
 where
     N: na::SimdRealField + Copy,
     N::Element: na::RealField + Copy,
@@ -399,18 +387,5 @@ where
     let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel);
     let bitangent1 = force_dir1.cross(&tangent1);
 
-    // Rotation such that: rot * tangent_fallback = tangent1
-    // (when projected in the tangent plane.) This is needed to ensure the
-    // warmstart impulse has the correct orientation. Indeed, at frame n + 1,
-    // we need to reapply the same impulse as we did in frame n. However the
-    // basis on which the tangent impulse is expresses may change at each frame
-    // (because the the relative linvel may change direction at each frame).
-    // So we need this rotation to:
-    // - Project the impulse back to the "reference" basis at after friction is resolved.
-    // - Project the old impulse on the new basis before the friction is resolved.
-    let rot = na::UnitComplex::new_unchecked(na::Complex::new(
-        tangent1.dot(&tangent_fallback),
-        bitangent1.dot(&tangent_fallback),
-    ));
-    ([tangent1, bitangent1], rot)
+    [tangent1, bitangent1]
 }

src/dynamics/solver/velocity_constraint_element.rs

@@ -9,48 +9,23 @@ pub(crate) struct VelocityConstraintTangentPart<N: SimdRealField + Copy> {
     pub gcross2: [AngVector<N>; DIM - 1],
     pub rhs: [N; DIM - 1],
     #[cfg(feature = "dim2")]
-    pub impulse: [N; DIM - 1],
+    pub impulse: na::Vector1<N>,
     #[cfg(feature = "dim3")]
     pub impulse: na::Vector2<N>,
     pub r: [N; DIM - 1],
 }
 
 impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     fn zero() -> Self {
         Self {
             gcross1: [na::zero(); DIM - 1],
             gcross2: [na::zero(); DIM - 1],
             rhs: [na::zero(); DIM - 1],
-            #[cfg(feature = "dim2")]
-            impulse: [na::zero(); DIM - 1],
-            #[cfg(feature = "dim3")]
             impulse: na::zero(),
             r: [na::zero(); DIM - 1],
         }
     }
 
-    #[inline]
-    pub fn warmstart(
-        &self,
-        tangents1: [&Vector<N>; DIM - 1],
-        im1: N,
-        im2: N,
-        mj_lambda1: &mut DeltaVel<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-        N::Element: SimdRealField + Copy,
-    {
-        for j in 0..DIM - 1 {
-            mj_lambda1.linear += tangents1[j] * (im1 * self.impulse[j]);
-            mj_lambda1.angular += self.gcross1[j] * self.impulse[j];
-
-            mj_lambda2.linear += tangents1[j] * (-im2 * self.impulse[j]);
-            mj_lambda2.angular += self.gcross2[j] * self.impulse[j];
-        }
-    }
-
     #[inline]
     pub fn solve(
         &mut self,
@@ -125,40 +100,23 @@ pub(crate) struct VelocityConstraintNormalPart<N: SimdRealField + Copy> {
     pub gcross1: AngVector<N>,
     pub gcross2: AngVector<N>,
     pub rhs: N,
+    pub rhs_wo_bias: N,
     pub impulse: N,
     pub r: N,
 }
 
 impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     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 warmstart(
-        &self,
-        dir1: &Vector<N>,
-        im1: N,
-        im2: N,
-        mj_lambda1: &mut DeltaVel<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-    {
-        mj_lambda1.linear += dir1 * (im1 * self.impulse);
-        mj_lambda1.angular += self.gcross1 * self.impulse;
-
-        mj_lambda2.linear += dir1 * (-im2 * self.impulse);
-        mj_lambda2.angular += self.gcross2 * self.impulse;
-    }
-
     #[inline]
     pub fn solve(
         &mut self,
@@ -193,7 +151,6 @@ pub(crate) struct VelocityConstraintElement<N: SimdRealField + Copy> {
 }
 
 impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     pub fn zero() -> Self {
         Self {
             normal_part: VelocityConstraintNormalPart::zero(),
@@ -201,35 +158,6 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
         }
     }
 
-    #[inline]
-    pub fn warmstart_group(
-        elements: &[Self],
-        dir1: &Vector<N>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im1: N,
-        im2: N,
-        mj_lambda1: &mut DeltaVel<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        Vector<N>: WBasis,
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-        N::Element: SimdRealField + Copy,
-    {
-        #[cfg(feature = "dim3")]
-        let tangents1 = [tangent1, &dir1.cross(&tangent1)];
-        #[cfg(feature = "dim2")]
-        let tangents1 = [&dir1.orthonormal_vector()];
-
-        for element in elements {
-            element
-                .tangent_part
-                .warmstart(tangents1, im1, im2, mj_lambda1, mj_lambda2);
-            element
-                .normal_part
-                .warmstart(dir1, im1, im2, mj_lambda1, mj_lambda2);
-        }
-    }
-
     #[inline]
     pub fn solve_group(
         elements: &mut [Self],
@@ -240,28 +168,34 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<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>,
         N::Element: SimdRealField + Copy,
     {
-        // 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);
+        // Solve penetration.
+        if solve_normal {
+            for element in elements.iter_mut() {
+                element
+                    .normal_part
+                    .solve(&dir1, im1, im2, mj_lambda1, mj_lambda2);
+            }
         }
 
-        // Solve penetration.
-        for element in elements.iter_mut() {
-            element
-                .normal_part
-                .solve(&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_constraint_wide.rs

@@ -10,7 +10,6 @@ use crate::math::{
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
 use crate::utils::{WAngularInertia, WCross, WDot};
-use na::SimdComplexField;
 use num::Zero;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
@@ -19,8 +18,6 @@ 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.
-    #[cfg(feature = "dim3")]
-    pub tangent_rot1: na::UnitComplex<SimdReal>, // Orientation of the tangent basis wrt. the reference basis.
     pub elements: [VelocityConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
     pub im1: SimdReal,
@@ -50,9 +47,9 @@ impl WVelocityConstraint {
         }
 
         let inv_dt = SimdReal::splat(params.inv_dt());
-        let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope);
         let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
-        let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
+        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
+        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
 
         let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
         let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
@@ -85,16 +82,12 @@ impl WVelocityConstraint {
         let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
         let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
 
-        let warmstart_multiplier =
-            SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]);
-        let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
         let num_active_contacts = manifolds[0].data.num_active_contacts();
 
         #[cfg(feature = "dim2")]
         let tangents1 = force_dir1.orthonormal_basis();
         #[cfg(feature = "dim3")]
-        let (tangents1, tangent_rot1) =
-            super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
+        let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
             let manifold_points =
@@ -105,8 +98,6 @@ impl WVelocityConstraint {
                 dir1: force_dir1,
                 #[cfg(feature = "dim3")]
                 tangent1: tangents1[0],
-                #[cfg(feature = "dim3")]
-                tangent_rot1,
                 elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im1,
                 im2,
@@ -130,18 +121,12 @@ impl WVelocityConstraint {
                 let tangent_velocity =
                     Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
 
-                let impulse =
-                    SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
-                let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]);
-
                 let dp1 = point - world_com1;
                 let dp2 = point - world_com2;
 
                 let vel1 = linvel1 + angvel1.gcross(dp1);
                 let vel2 = linvel2 + angvel2.gcross(dp2);
 
-                let warmstart_correction;
-
                 constraint.limit = friction;
                 constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
 
@@ -153,39 +138,25 @@ impl WVelocityConstraint {
                     let r = SimdReal::splat(1.0)
                         / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
-                    let mut rhs =
+                    let mut rhs_wo_bias =
                         (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
-                    rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
-                    rhs *= is_bouncy + is_resting * velocity_solve_fraction;
-                    rhs +=
-                        dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting);
-                    warmstart_correction = (warmstart_correction_slope
-                        / (rhs - prev_rhs).simd_abs())
-                    .simd_min(warmstart_coeff);
+                    rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt;
+                    rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
+                    let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
+                        * (erp_inv_dt/* * is_resting */);
 
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
-                        rhs,
-                        impulse: impulse * warmstart_correction,
+                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs_wo_bias,
+                        impulse: na::zero(),
                         r,
                     };
                 }
 
                 // tangent parts.
-                #[cfg(feature = "dim2")]
-                let impulse = [SimdReal::from(gather![
-                    |ii| manifold_points[ii][k].warmstart_tangent_impulse
-                ]) * warmstart_correction];
-
-                #[cfg(feature = "dim3")]
-                let impulse = tangent_rot1
-                    * na::Vector2::from(gather![
-                        |ii| manifold_points[ii][k].warmstart_tangent_impulse
-                    ])
-                    * warmstart_correction;
-
-                constraint.elements[k].tangent_part.impulse = impulse;
+                constraint.elements[k].tangent_part.impulse = na::zero();
 
                 for j in 0..DIM - 1 {
                     let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
@@ -210,43 +181,12 @@ impl WVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
-            ]),
-        };
-
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        VelocityConstraintElement::warmstart_group(
-            &self.elements[..self.num_contacts as usize],
-            &self.dir1,
-            #[cfg(feature = "dim3")]
-            &self.tangent1,
-            self.im1,
-            self.im2,
-            &mut mj_lambda1,
-            &mut mj_lambda2,
-        );
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
-            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
-        }
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    pub fn solve(
+        &mut self,
+        mj_lambdas: &mut [DeltaVel<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]),
             angular: AngVector::from(gather![
@@ -271,6 +211,8 @@ impl WVelocityConstraint {
             self.limit,
             &mut mj_lambda1,
             &mut mj_lambda2,
+            solve_normal,
+            solve_friction,
         );
 
         for ii in 0..SIMD_WIDTH {
@@ -286,19 +228,15 @@ impl WVelocityConstraint {
     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();
-            let rhs: [_; SIMD_WIDTH] = self.elements[k].normal_part.rhs.into();
             #[cfg(feature = "dim2")]
             let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
             #[cfg(feature = "dim3")]
-            let tangent_impulses = self
-                .tangent_rot1
-                .inverse_transform_vector(&self.elements[k].tangent_part.impulse);
+            let tangent_impulses = self.elements[k].tangent_part.impulse;
 
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
                 let contact_id = self.manifold_contact_id[k][ii];
                 let active_contact = &mut manifold.points[contact_id as usize];
-                active_contact.data.rhs = rhs[ii];
                 active_contact.data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]
@@ -312,4 +250,10 @@ impl WVelocityConstraint {
             }
         }
     }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        for elt in &mut self.elements {
+            elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
+        }
+    }
 }

src/dynamics/solver/velocity_ground_constraint.rs

@@ -21,8 +21,6 @@ pub(crate) struct VelocityGroundConstraint {
     pub limit: Real,
     pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS],
 
-    #[cfg(feature = "dim3")]
-    pub tangent_rot1: na::UnitComplex<Real>, // Orientation of the tangent basis wrt. the reference basis.
     pub manifold_id: ContactManifoldIndex,
     pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
@@ -42,7 +40,7 @@ impl VelocityGroundConstraint {
             + ComponentSet<RigidBodyMassProps>,
     {
         let inv_dt = params.inv_dt();
-        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        let erp_inv_dt = params.erp_inv_dt();
 
         let mut handle1 = manifold.data.rigid_body1;
         let mut handle2 = manifold.data.rigid_body2;
@@ -69,11 +67,10 @@ impl VelocityGroundConstraint {
         #[cfg(feature = "dim2")]
         let tangents1 = force_dir1.orthonormal_basis();
         #[cfg(feature = "dim3")]
-        let (tangents1, tangent_rot1) =
+        let tangents1 =
             super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
 
         let mj_lambda2 = ids2.active_set_offset;
-        let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
 
         for (_l, manifold_points) in manifold
             .data
@@ -86,8 +83,6 @@ impl VelocityGroundConstraint {
                 dir1: force_dir1,
                 #[cfg(feature = "dim3")]
                 tangent1: tangents1[0],
-                #[cfg(feature = "dim3")]
-                tangent_rot1,
                 elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im2: mprops2.effective_inv_mass,
                 limit: 0.0,
@@ -106,7 +101,7 @@ impl VelocityGroundConstraint {
             // avoid spurious copying.
             // Is this optimization beneficial when targeting non-WASM platforms?
             //
-            // NOTE: joints have the same problem, but it is not easy to refactor the code that way
+            // 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 push {
@@ -133,7 +128,6 @@ impl VelocityGroundConstraint {
                 #[cfg(feature = "dim3")]
                 {
                     constraint.tangent1 = tangents1[0];
-                    constraint.tangent_rot1 = tangent_rot1;
                 }
                 constraint.im2 = mprops2.effective_inv_mass;
                 constraint.limit = 0.0;
@@ -149,7 +143,6 @@ impl VelocityGroundConstraint {
                 let dp1 = manifold_point.point - world_com1;
                 let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
                 let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
-                let warmstart_correction;
 
                 constraint.limit = manifold_point.friction;
                 constraint.manifold_contact_id[k] = manifold_point.contact_id;
@@ -165,33 +158,27 @@ impl VelocityGroundConstraint {
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
                     let is_resting = 1.0 - is_bouncy;
 
-                    let mut rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
-                    rhs += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs *= is_bouncy + is_resting * params.velocity_solve_fraction;
-                    rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0);
-                    warmstart_correction = (params.warmstart_correction_slope
-                        / (rhs - manifold_point.prev_rhs).abs())
-                    .min(warmstart_coeff);
+                    rhs_wo_bias +=
+                        (manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt;
+                    rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    let rhs_bias = /* is_resting
+                        * */ erp_inv_dt
+                        * (manifold_point.dist + params.allowed_linear_error).min(0.0);
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2,
-                        rhs,
-                        impulse: manifold_point.warmstart_impulse * warmstart_correction,
+                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs_wo_bias,
+                        impulse: 0.0,
                         r,
                     };
                 }
 
                 // Tangent parts.
                 {
-                    #[cfg(feature = "dim3")]
-                    let impulse = tangent_rot1
-                        * manifold_points[k].warmstart_tangent_impulse
-                        * warmstart_correction;
-                    #[cfg(feature = "dim2")]
-                    let impulse =
-                        [manifold_points[k].warmstart_tangent_impulse * warmstart_correction];
-                    constraint.elements[k].tangent_part.impulse = impulse;
+                    constraint.elements[k].tangent_part.impulse = na::zero();
 
                     for j in 0..DIM - 1 {
                         let gcross2 = mprops2
@@ -219,23 +206,12 @@ impl VelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
-        let mut mj_lambda2 = DeltaVel::zero();
-
-        VelocityGroundConstraintElement::warmstart_group(
-            &self.elements[..self.num_contacts as usize],
-            &self.dir1,
-            #[cfg(feature = "dim3")]
-            &self.tangent1,
-            self.im2,
-            &mut mj_lambda2,
-        );
-
-        mj_lambdas[self.mj_lambda2 as usize].linear += mj_lambda2.linear;
-        mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular;
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    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(
@@ -246,6 +222,8 @@ impl VelocityGroundConstraint {
             self.im2,
             self.limit,
             &mut mj_lambda2,
+            solve_normal,
+            solve_friction,
         );
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -259,7 +237,6 @@ impl VelocityGroundConstraint {
             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;
-            active_contact.data.rhs = self.elements[k].normal_part.rhs;
 
             #[cfg(feature = "dim2")]
             {
@@ -267,10 +244,14 @@ impl VelocityGroundConstraint {
             }
             #[cfg(feature = "dim3")]
             {
-                active_contact.data.tangent_impulse = self
-                    .tangent_rot1
-                    .inverse_transform_vector(&self.elements[k].tangent_part.impulse);
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
             }
         }
     }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        for elt in &mut self.elements {
+            elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
+        }
+    }
 }

src/dynamics/solver/velocity_ground_constraint_element.rs

@@ -8,7 +8,7 @@ pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
     pub gcross2: [AngVector<N>; DIM - 1],
     pub rhs: [N; DIM - 1],
     #[cfg(feature = "dim2")]
-    pub impulse: [N; DIM - 1],
+    pub impulse: na::Vector1<N>,
     #[cfg(feature = "dim3")]
     pub impulse: na::Vector2<N>,
     pub r: [N; DIM - 1],
@@ -20,27 +20,11 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
         Self {
             gcross2: [na::zero(); DIM - 1],
             rhs: [na::zero(); DIM - 1],
-            #[cfg(feature = "dim2")]
-            impulse: [na::zero(); DIM - 1],
-            #[cfg(feature = "dim3")]
             impulse: na::zero(),
             r: [na::zero(); DIM - 1],
         }
     }
 
-    #[inline]
-    pub fn warmstart(
-        &self,
-        tangents1: [&Vector<N>; DIM - 1],
-        im2: N,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) {
-        for j in 0..DIM - 1 {
-            mj_lambda2.linear += tangents1[j] * (-im2 * self.impulse[j]);
-            mj_lambda2.angular += self.gcross2[j] * self.impulse[j];
-        }
-    }
-
     #[inline]
     pub fn solve(
         &mut self,
@@ -99,6 +83,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
 pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
     pub gcross2: AngVector<N>,
     pub rhs: N,
+    pub rhs_wo_bias: N,
     pub impulse: N,
     pub r: N,
 }
@@ -109,17 +94,12 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
         Self {
             gcross2: na::zero(),
             rhs: na::zero(),
+            rhs_wo_bias: na::zero(),
             impulse: na::zero(),
             r: na::zero(),
         }
     }
 
-    #[inline]
-    pub fn warmstart(&self, dir1: &Vector<N>, im2: N, mj_lambda2: &mut DeltaVel<N>) {
-        mj_lambda2.linear += dir1 * (-im2 * self.impulse);
-        mj_lambda2.angular += self.gcross2 * self.impulse;
-    }
-
     #[inline]
     pub fn solve(&mut self, dir1: &Vector<N>, im2: N, mj_lambda2: &mut DeltaVel<N>)
     where
@@ -151,29 +131,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
         }
     }
 
-    #[inline]
-    pub fn warmstart_group(
-        elements: &[Self],
-        dir1: &Vector<N>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im2: N,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        Vector<N>: WBasis,
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-        N::Element: SimdRealField + Copy,
-    {
-        #[cfg(feature = "dim3")]
-        let tangents1 = [tangent1, &dir1.cross(&tangent1)];
-        #[cfg(feature = "dim2")]
-        let tangents1 = [&dir1.orthonormal_vector()];
-
-        for element in elements {
-            element.normal_part.warmstart(dir1, im2, mj_lambda2);
-            element.tangent_part.warmstart(tangents1, im2, mj_lambda2);
-        }
-    }
-
     #[inline]
     pub fn solve_group(
         elements: &mut [Self],
@@ -182,26 +139,32 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
         im2: 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>,
         N::Element: SimdRealField + Copy,
     {
-        // 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);
+        // Solve penetration.
+        if solve_normal {
+            for element in elements.iter_mut() {
+                element.normal_part.solve(&dir1, im2, mj_lambda2);
+            }
         }
 
-        // Solve penetration.
-        for element in elements.iter_mut() {
-            element.normal_part.solve(&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_ground_constraint_wide.rs

@@ -11,7 +11,6 @@ use crate::math::{
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
 use crate::utils::{WAngularInertia, WCross, WDot};
-use na::SimdComplexField;
 use num::Zero;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
@@ -20,8 +19,6 @@ 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.
-    #[cfg(feature = "dim3")]
-    pub tangent_rot1: na::UnitComplex<SimdReal>, // Orientation of the tangent basis wrt. the reference basis.
     pub elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
     pub im2: SimdReal,
@@ -46,7 +43,8 @@ impl WVelocityGroundConstraint {
     {
         let inv_dt = SimdReal::splat(params.inv_dt());
         let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
-        let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
+        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
+        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
 
         let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
         let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
@@ -95,17 +93,12 @@ impl WVelocityGroundConstraint {
 
         let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
 
-        let warmstart_multiplier =
-            SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]);
-        let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
-        let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope);
         let num_active_contacts = manifolds[0].data.num_active_contacts();
 
         #[cfg(feature = "dim2")]
         let tangents1 = force_dir1.orthonormal_basis();
         #[cfg(feature = "dim3")]
-        let (tangents1, tangent_rot1) =
-            super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
+        let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
             let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
@@ -115,8 +108,6 @@ impl WVelocityGroundConstraint {
                 dir1: force_dir1,
                 #[cfg(feature = "dim3")]
                 tangent1: tangents1[0],
-                #[cfg(feature = "dim3")]
-                tangent_rot1,
                 elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im2,
                 limit: SimdReal::splat(0.0),
@@ -138,15 +129,11 @@ impl WVelocityGroundConstraint {
                 let tangent_velocity =
                     Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
 
-                let impulse =
-                    SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
-                let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]);
                 let dp1 = point - world_com1;
                 let dp2 = point - world_com2;
 
                 let vel1 = linvel1 + angvel1.gcross(dp1);
                 let vel2 = linvel2 + angvel2.gcross(dp2);
-                let warmstart_correction;
 
                 constraint.limit = friction;
                 constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
@@ -157,36 +144,24 @@ impl WVelocityGroundConstraint {
 
                     let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
-                    let mut rhs =
+                    let mut rhs_wo_bias =
                         (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
-                    rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
-                    rhs *= is_bouncy + is_resting * velocity_solve_fraction;
-                    rhs +=
-                        dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting);
-                    warmstart_correction = (warmstart_correction_slope
-                        / (rhs - prev_rhs).simd_abs())
-                    .simd_min(warmstart_coeff);
+                    rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt;
+                    rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
+                    let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
+                        * (erp_inv_dt/* * is_resting */);
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2,
-                        rhs,
-                        impulse: impulse * warmstart_correction,
+                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs_wo_bias,
+                        impulse: na::zero(),
                         r,
                     };
                 }
 
                 // tangent parts.
-                #[cfg(feature = "dim2")]
-                let impulse = [SimdReal::from(gather![
-                    |ii| manifold_points[ii][k].warmstart_tangent_impulse
-                ]) * warmstart_correction];
-                #[cfg(feature = "dim3")]
-                let impulse = tangent_rot1
-                    * na::Vector2::from(gather![
-                        |ii| manifold_points[ii][k].warmstart_tangent_impulse
-                    ])
-                    * warmstart_correction;
-                constraint.elements[k].tangent_part.impulse = impulse;
+                constraint.elements[k].tangent_part.impulse = na::zero();
 
                 for j in 0..DIM - 1 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
@@ -208,30 +183,12 @@ impl WVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&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]),
-            angular: AngVector::from(gather![
-                |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
-            ]),
-        };
-
-        VelocityGroundConstraintElement::warmstart_group(
-            &self.elements[..self.num_contacts as usize],
-            &self.dir1,
-            #[cfg(feature = "dim3")]
-            &self.tangent1,
-            self.im2,
-            &mut mj_lambda2,
-        );
-
-        for ii in 0..SIMD_WIDTH {
-            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
-            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
-        }
-    }
-
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    pub fn solve(
+        &mut self,
+        mj_lambdas: &mut [DeltaVel<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]),
             angular: AngVector::from(gather![
@@ -247,6 +204,8 @@ impl WVelocityGroundConstraint {
             self.im2,
             self.limit,
             &mut mj_lambda2,
+            solve_normal,
+            solve_friction,
         );
 
         for ii in 0..SIMD_WIDTH {
@@ -258,20 +217,16 @@ impl WVelocityGroundConstraint {
     // 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]) {
         for k in 0..self.num_contacts as usize {
-            let rhs: [_; SIMD_WIDTH] = self.elements[k].normal_part.rhs.into();
             let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
             #[cfg(feature = "dim2")]
             let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
             #[cfg(feature = "dim3")]
-            let tangent_impulses = self
-                .tangent_rot1
-                .inverse_transform_vector(&self.elements[k].tangent_part.impulse);
+            let tangent_impulses = self.elements[k].tangent_part.impulse;
 
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
                 let contact_id = self.manifold_contact_id[k][ii];
                 let active_contact = &mut manifold.points[contact_id as usize];
-                active_contact.data.rhs = rhs[ii];
                 active_contact.data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]
@@ -285,4 +240,10 @@ impl WVelocityGroundConstraint {
             }
         }
     }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        for elt in &mut self.elements {
+            elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
+        }
+    }
 }

src/dynamics/solver/velocity_solver.rs

@@ -1,22 +1,28 @@
 use super::AnyJointVelocityConstraint;
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
+use crate::dynamics::solver::GenericVelocityConstraint;
 use crate::dynamics::{
     solver::{AnyVelocityConstraint, DeltaVel},
-    IntegrationParameters, JointGraphEdge, RigidBodyForces, RigidBodyVelocity,
+    IntegrationParameters, JointGraphEdge, MultibodyJointSet, RigidBodyForces, RigidBodyType,
+    RigidBodyVelocity,
 };
 use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyMassProps};
 use crate::geometry::ContactManifold;
 use crate::math::Real;
+use crate::prelude::{RigidBodyActivation, RigidBodyDamping, RigidBodyPosition};
 use crate::utils::WAngularInertia;
+use na::DVector;
 
 pub(crate) struct VelocitySolver {
     pub mj_lambdas: Vec<DeltaVel<Real>>,
+    pub generic_mj_lambdas: DVector<Real>,
 }
 
 impl VelocitySolver {
     pub fn new() -> Self {
         Self {
             mj_lambdas: Vec::new(),
+            generic_mj_lambdas: DVector::zeros(0),
         }
     }
 
@@ -26,20 +32,31 @@ impl VelocitySolver {
         params: &IntegrationParameters,
         islands: &IslandManager,
         bodies: &mut Bodies,
+        multibodies: &mut MultibodyJointSet,
         manifolds_all: &mut [&mut ContactManifold],
         joints_all: &mut [JointGraphEdge],
         contact_constraints: &mut [AnyVelocityConstraint],
+        generic_contact_constraints: &mut [GenericVelocityConstraint],
+        generic_contact_jacobians: &DVector<Real>,
         joint_constraints: &mut [AnyJointVelocityConstraint],
+        generic_joint_jacobians: &DVector<Real>,
     ) where
         Bodies: ComponentSet<RigidBodyForces>
             + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>
             + ComponentSetMut<RigidBodyVelocity>
-            + ComponentSet<RigidBodyMassProps>,
+            + ComponentSetMut<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyPosition>
+            + ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyDamping>,
     {
         self.mj_lambdas.clear();
         self.mj_lambdas
             .resize(islands.active_island(island_id).len(), DeltaVel::zero());
 
+        let total_multibodies_ndofs = multibodies.multibodies.iter().map(|m| m.1.ndofs()).sum();
+        self.generic_mj_lambdas = DVector::zeros(total_multibodies_ndofs);
+
         // Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc):
         for handle in islands.active_island(island_id) {
             let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
@@ -53,43 +70,196 @@ impl VelocitySolver {
             dvel.linear += forces.force * (mprops.effective_inv_mass * params.dt);
         }
 
-        /*
-         * Warmstart constraints.
-         */
-        for constraint in &*joint_constraints {
-            constraint.warmstart(&mut self.mj_lambdas[..]);
-        }
-
-        for constraint in &*contact_constraints {
-            constraint.warmstart(&mut self.mj_lambdas[..]);
+        for (_, multibody) in multibodies.multibodies.iter_mut() {
+            let mut mj_lambdas = self
+                .generic_mj_lambdas
+                .rows_mut(multibody.solver_id, multibody.ndofs());
+            mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
         }
 
         /*
          * Solve constraints.
          */
-        for _ in 0..params.max_velocity_iterations {
+        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 constraint in &mut *joint_constraints {
-                constraint.solve(&mut self.mj_lambdas[..]);
+                constraint.solve(
+                    generic_joint_jacobians,
+                    &mut self.mj_lambdas[..],
+                    &mut self.generic_mj_lambdas,
+                );
             }
 
             for constraint in &mut *contact_constraints {
-                constraint.solve(&mut self.mj_lambdas[..]);
+                constraint.solve(&mut self.mj_lambdas[..], true, solve_friction);
+            }
+
+            for constraint in &mut *generic_contact_constraints {
+                constraint.solve(
+                    generic_contact_jacobians,
+                    &mut self.mj_lambdas[..],
+                    &mut self.generic_mj_lambdas,
+                    true,
+                    solve_friction,
+                );
             }
         }
 
-        // Update 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
+            } else {
+                0
+            };
+
+        for _ in 0..remaining_friction_iterations {
+            for constraint in &mut *contact_constraints {
+                constraint.solve(&mut self.mj_lambdas[..], false, true);
+            }
+
+            for constraint in &mut *generic_contact_constraints {
+                constraint.solve(
+                    generic_contact_jacobians,
+                    &mut self.mj_lambdas[..],
+                    &mut self.generic_mj_lambdas,
+                    false,
+                    true,
+                );
+            }
+        }
+
+        // Update positions.
         for handle in islands.active_island(island_id) {
-            let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = bodies.index_bundle(handle.0);
+            if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
+                let multibody = multibodies
+                    .get_multibody_mut_internal(link.multibody)
+                    .unwrap();
 
-            let dvel = self.mj_lambdas[ids.active_set_offset];
-            let dangvel = mprops
-                .effective_world_inv_inertia_sqrt
-                .transform_vector(dvel.angular);
+                if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
+                    let mj_lambdas = self
+                        .generic_mj_lambdas
+                        .rows(multibody.solver_id, multibody.ndofs());
+                    let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations.
+                    multibody.velocities += mj_lambdas;
+                    multibody.integrate_next(params.dt);
+                    multibody.forward_kinematics_next(bodies, false);
 
-            bodies.map_mut_internal(handle.0, |vels| {
-                vels.linvel += dvel.linear;
-                vels.angvel += dangvel;
-            });
+                    if params.max_stabilization_iterations > 0 {
+                        multibody.velocities = prev_vels;
+                    }
+                }
+            } else {
+                let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
+                    bodies.index_bundle(handle.0);
+
+                let dvel = self.mj_lambdas[ids.active_set_offset];
+                let dangvel = mprops
+                    .effective_world_inv_inertia_sqrt
+                    .transform_vector(dvel.angular);
+
+                // Update positions.
+                let (poss, vels, damping, mprops): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyDamping,
+                    &RigidBodyMassProps,
+                ) = bodies.index_bundle(handle.0);
+
+                let mut new_poss = *poss;
+                let mut new_vels = *vels;
+                new_vels.linvel += dvel.linear;
+                new_vels.angvel += dangvel;
+                new_vels = new_vels.apply_damping(params.dt, damping);
+                new_poss.next_position =
+                    new_vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com);
+                bodies.set_internal(handle.0, new_poss);
+
+                if params.max_stabilization_iterations == 0 {
+                    bodies.set_internal(handle.0, new_vels);
+                }
+            }
+        }
+
+        if params.max_stabilization_iterations > 0 {
+            for joint in &mut *joint_constraints {
+                joint.remove_bias_from_rhs();
+            }
+            for constraint in &mut *contact_constraints {
+                constraint.remove_bias_from_rhs();
+            }
+            for constraint in &mut *generic_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(&mut self.mj_lambdas[..], true, true);
+                }
+
+                for constraint in &mut *generic_contact_constraints {
+                    constraint.solve(
+                        generic_contact_jacobians,
+                        &mut self.mj_lambdas[..],
+                        &mut self.generic_mj_lambdas,
+                        true,
+                        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 (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
+                        bodies.index_bundle(handle.0);
+
+                    let dvel = self.mj_lambdas[ids.active_set_offset];
+                    let dangvel = mprops
+                        .effective_world_inv_inertia_sqrt
+                        .transform_vector(dvel.angular);
+
+                    // let mut curr_vel_pseudo_energy = 0.0;
+                    bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
+                        // curr_vel_pseudo_energy = vels.pseudo_kinetic_energy();
+                        vels.linvel += dvel.linear;
+                        vels.angvel += dangvel;
+                    });
+
+                    // let impulse_vel_pseudo_energy = RigidBodyVelocity {
+                    //     linvel: dvel.linear,
+                    //     angvel: dangvel,
+                    // }
+                    // .pseudo_kinetic_energy();
+                    //
+                    // bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
+                    //     activation.energy =
+                    //         impulse_vel_pseudo_energy.max(curr_vel_pseudo_energy / 5.0);
+                    // });
+                }
+            }
         }
 
         // Write impulses back into the manifold structures.
@@ -100,5 +270,9 @@ impl VelocitySolver {
         for constraint in &*contact_constraints {
             constraint.writeback_impulses(manifolds_all);
         }
+
+        for constraint in &*generic_contact_constraints {
+            constraint.writeback_impulses(manifolds_all);
+        }
     }
 }