Implement multibody joints and the new solver

27 files changed, 3087 insertions, 8576 deletions

diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
deleted file mode 100644
index 0dfdb86..0000000
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
+++ /dev/null
@@ -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;
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
deleted file mode 100644
index ea462ed..0000000
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
+++ /dev/null
@@ -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);
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
deleted file mode 100644
index 5abd726..0000000
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
+++ /dev/null
@@ -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_im