Merge pull request #579 from dimforge/joints-improvements

Feat: implement a "small-steps" velocity-based constraints solver + joint improvements

8 files changed, 1939 insertions, 1218 deletions

diff --git a/src/dynamics/solver/joint_constraint/any_joint_constraint.rs b/src/dynamics/solver/joint_constraint/any_joint_constraint.rs
new file mode 100644
index 0000000..8cbd438
--- /dev/null
+++ b/src/dynamics/solver/joint_constraint/any_joint_constraint.rs
@@ -0,0 +1,97 @@
+use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{
+    JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
+};
+use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
+    JointOneBodyConstraint, JointTwoBodyConstraint,
+};
+use crate::dynamics::solver::{AnyConstraintMut, ConstraintTypes};
+use crate::dynamics::JointGraphEdge;
+use crate::math::Real;
+use na::DVector;
+
+use crate::dynamics::solver::joint_constraint::joint_generic_constraint_builder::{
+    JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder,
+};
+use crate::dynamics::solver::solver_vel::SolverVel;
+#[cfg(feature = "simd-is-enabled")]
+use crate::{
+    dynamics::solver::joint_constraint::joint_constraint_builder::{
+        JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
+    },
+    math::{SimdReal, SIMD_WIDTH},
+};
+
+use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
+    JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder,
+};
+
+pub struct JointConstraintTypes;
+
+impl<'a> AnyConstraintMut<'a, JointConstraintTypes> {
+    pub fn remove_bias(&mut self) {
+        match self {
+            Self::OneBody(c) => c.remove_bias_from_rhs(),
+            Self::TwoBodies(c) => c.remove_bias_from_rhs(),
+            Self::GenericOneBody(c) => c.remove_bias_from_rhs(),
+            Self::GenericTwoBodies(c) => c.remove_bias_from_rhs(),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.remove_bias_from_rhs(),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.remove_bias_from_rhs(),
+        }
+    }
+
+    pub fn solve(
+        &mut self,
+        generic_jacobians: &DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        match self {
+            Self::OneBody(c) => c.solve(solver_vels),
+            Self::TwoBodies(c) => c.solve(solver_vels),
+            Self::GenericOneBody(c) => c.solve(generic_jacobians, solver_vels, generic_solver_vels),
+            Self::GenericTwoBodies(c) => {
+                c.solve(generic_jacobians, solver_vels, generic_solver_vels)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.solve(solver_vels),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.solve(solver_vels),
+        }
+    }
+
+    pub fn writeback_impulses(&mut self, joints_all: &mut [JointGraphEdge]) {
+        match self {
+            Self::OneBody(c) => c.writeback_impulses(joints_all),
+            Self::TwoBodies(c) => c.writeback_impulses(joints_all),
+            Self::GenericOneBody(c) => c.writeback_impulses(joints_all),
+            Self::GenericTwoBodies(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.writeback_impulses(joints_all),
+        }
+    }
+}
+
+impl ConstraintTypes for JointConstraintTypes {
+    type OneBody = JointOneBodyConstraint<Real, 1>;
+    type TwoBodies = JointTwoBodyConstraint<Real, 1>;
+    type GenericOneBody = JointGenericOneBodyConstraint;
+    type GenericTwoBodies = JointGenericTwoBodyConstraint;
+
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdOneBody = JointOneBodyConstraint<SimdReal, SIMD_WIDTH>;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdTwoBodies = JointTwoBodyConstraint<SimdReal, SIMD_WIDTH>;
+
+    type BuilderOneBody = JointOneBodyConstraintBuilder;
+    type BuilderTwoBodies = JointTwoBodyConstraintBuilder;
+    type GenericBuilderOneBody = JointGenericOneBodyConstraintBuilder;
+    type GenericBuilderTwoBodies = JointGenericTwoBodyConstraintBuilder;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdBuilderOneBody = JointOneBodyConstraintBuilderSimd;
+    #[cfg(feature = "simd-is-enabled")]
+    type SimdBuilderTwoBodies = JointTwoBodyConstraintBuilderSimd;
+}
diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
deleted file mode 100644
index 962602f..0000000
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ /dev/null
@@ -1,540 +0,0 @@
-use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{
-    JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
-};
-use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
-    JointVelocityConstraint, JointVelocityGroundConstraint, SolverBody,
-};
-use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{
-    ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
-};
-use crate::math::{Real, SPATIAL_DIM};
-use crate::prelude::MultibodyJointSet;
-use na::DVector;
-
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::{Isometry, SimdReal, SIMD_WIDTH};
-
-#[cfg(feature = "parallel")]
-use crate::dynamics::JointAxesMask;
-
-pub enum AnyJointVelocityConstraint {
-    JointConstraint(JointVelocityConstraint<Real, 1>),
-    JointGroundConstraint(JointVelocityGroundConstraint<Real, 1>),
-    JointGenericConstraint(JointGenericVelocityConstraint),
-    JointGenericGroundConstraint(JointGenericVelocityGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    JointConstraintSimd(JointVelocityConstraint<SimdReal, SIMD_WIDTH>),
-    #[cfg(feature = "simd-is-enabled")]
-    JointGroundConstraintSimd(JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH>),
-    Empty,
-}
-
-impl AnyJointVelocityConstraint {
-    #[cfg(feature = "parallel")]
-    pub fn num_active_constraints_and_jacobian_lines(joint: &ImpulseJoint) -> (usize, usize) {
-        let joint = &joint.data;
-        let locked_axes = joint.locked_axes.bits();
-        let motor_axes = joint.motor_axes.bits() & !locked_axes;
-        let limit_axes = joint.limit_axes.bits() & !locked_axes;
-        let coupled_axes = joint.coupled_axes.bits();
-
-        let num_constraints = (motor_axes & !coupled_axes).count_ones() as usize
-            + ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
-            + ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize
-            + locked_axes.count_ones() as usize
-            + (limit_axes & !coupled_axes).count_ones() as usize
-            + ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
-            + ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize;
-        (num_constraints, num_constraints)
-    }
-
-    pub fn from_joint(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        joint: &ImpulseJoint,
-        bodies: &RigidBodySet,
-        multibodies: &MultibodyJointSet,
-        j_id: &mut usize,
-        jacobians: &mut DVector<Real>,
-        out: &mut Vec<Self>,
-        insert_at: Option<usize>,
-    ) {
-        let local_frame1 = joint.data.local_frame1;
-        let local_frame2 = joint.data.local_frame2;
-        let rb1 = &bodies[joint.body1];
-        let rb2 = &bodies[joint.body2];
-        let frame1 = rb1.pos.position * local_frame1;
-        let frame2 = rb2.pos.position * local_frame2;
-
-        let body1 = SolverBody {
-            linvel: rb1.vels.linvel,
-            angvel: rb1.vels.angvel,
-            im: rb1.mprops.effective_inv_mass,
-            sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
-            world_com: rb1.mprops.world_com,
-            mj_lambda: [rb1.ids.active_set_offset],
-        };
-        let body2 = SolverBody {
-            linvel: rb2.vels.linvel,
-            angvel: rb2.vels.angvel,
-            im: rb2.mprops.effective_inv_mass,
-            sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
-            world_com: rb2.mprops.world_com,
-            mj_lambda: [rb2.ids.active_set_offset],
-        };
-
-        let mb1 = multibodies
-            .rigid_body_link(joint.body1)
-            .map(|link| (&multibodies[link.multibody], link.id));
-        let mb2 = multibodies
-            .rigid_body_link(joint.body2)
-            .map(|link| (&multibodies[link.multibody], link.id));
-
-        if mb1.is_some() || mb2.is_some() {
-            let multibodies_ndof = mb1.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM)
-                + mb2.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM);
-
-            if multibodies_ndof == 0 {
-                // Both multibodies are fixed, don’t generate any constraint.
-                return;
-            }
-
-            // For each solver contact we generate up to SPATIAL_DIM constraints, and each
-            // constraints appends the multibodies jacobian and weighted jacobians.
-            // Also note that for impulse_joints, the rigid-bodies will also add their jacobians
-            // to the generic DVector.
-            // TODO: is this count correct when we take both motors and limits into account?
-            let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
-
-            if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
-                jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
-            }
-
-            // TODO: find a way to avoid the temporary buffer.
-            let mut out_tmp = [JointGenericVelocityConstraint::invalid(); 12];
-            let out_tmp_len = JointGenericVelocityConstraint::lock_axes(
-                params,
-                joint_id,
-                &body1,
-                &body2,
-                mb1,
-                mb2,
-                &frame1,
-                &frame2,
-                &joint.data,
-                jacobians,
-                j_id,
-                &mut out_tmp,
-            );
-
-            if let Some(at) = insert_at {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out[at + i] = AnyJointVelocityConstraint::JointGenericConstraint(c);
-                }
-            } else {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
-                }
-            }
-        } else {
-            // TODO: find a way to avoid the temporary buffer.
-            let mut out_tmp = [JointVelocityConstraint::invalid(); 12];
-            let out_tmp_len = JointVelocityConstraint::<Real, 1>::lock_axes(
-                params,
-                joint_id,
-                &body1,
-                &body2,
-                &frame1,
-                &frame2,
-                &joint.data,
-                &mut out_tmp,
-            );
-
-            if let Some(at) = insert_at {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out[at + i] = AnyJointVelocityConstraint::JointConstraint(c);
-                }
-            } else {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out.push(AnyJointVelocityConstraint::JointConstraint(c));
-                }
-            }
-        }
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
-        bodies: &RigidBodySet,
-        out: &mut Vec<Self>,
-        insert_at: Option<usize>,
-    ) {
-        use crate::dynamics::{
-            RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
-        };
-
-        let rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ) = (
-            gather![|ii| &bodies[impulse_joints[ii].body1].pos],
-            gather![|ii| &bodies[impulse_joints[ii].body1].vels],
-            gather![|ii| &bodies[impulse_joints[ii].body1].mprops],
-            gather![|ii| &bodies[impulse_joints[ii].body1].ids],
-        );
-        let rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ) = (
-            gather![|ii| &bodies[impulse_joints[ii].body2].pos],
-            gather![|ii| &bodies[impulse_joints[ii].body2].vels],
-            gather![|ii| &bodies[impulse_joints[ii].body2].mprops],
-            gather![|ii| &bodies[impulse_joints[ii].body2].ids],
-        );
-
-        let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1;
-        let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2;
-        let pos1: Isometry<SimdReal> = gather![|ii| rb_pos1[ii].position].into();
-        let pos2: Isometry<SimdReal> = gather![|ii| rb_pos2[ii].position].into();
-
-        let local_frame1: Isometry<SimdReal> =
-            gather![|ii| impulse_joints[ii].data.local_frame1].into();
-        let local_frame2: Isometry<SimdReal> =
-            gather![|ii| impulse_joints[ii].data.local_frame2].into();
-
-        let frame1 = pos1 * local_frame1;
-        let frame2 = pos2 * local_frame2;
-
-        let body1: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
-            linvel: gather![|ii| rb_vel1[ii].linvel].into(),
-            angvel: gather![|ii| rb_vel1[ii].angvel].into(),
-            im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(),
-            sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(),
-            world_com: gather![|ii| rb_mprops1[ii].world_com].into(),
-            mj_lambda: gather![|ii| rb_ids1[ii].active_set_offset],
-        };
-        let body2: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
-            linvel: gather![|ii| rb_vel2[ii].linvel].into(),
-            angvel: gather![|ii| rb_vel2[ii].angvel].into(),
-            im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(),
-            sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(),
-            world_com: gather![|ii| rb_mprops2[ii].world_com].into(),
-            mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset],
-        };
-
-        // TODO: find a way to avoid the temporary buffer.
-        let mut out_tmp = [JointVelocityConstraint::invalid(); 12];
-        let out_tmp_len = JointVelocityConstraint::<SimdReal, SIMD_WIDTH>::lock_axes(
-            params,
-            joint_id,
-            &body1,
-            &body2,
-            &frame1,
-            &frame2,
-            impulse_joints[0].data.locked_axes.bits(),
-            &mut out_tmp,
-        );
-
-        if let Some(at) = insert_at {
-            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                out[at + i] = AnyJointVelocityConstraint::JointConstraintSimd(c);
-            }
-        } else {
-            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
-            }
-        }
-    }
-
-    pub fn from_joint_ground(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        joint: &ImpulseJoint,
-        bodies: &RigidBodySet,
-        multibodies: &MultibodyJointSet,
-        j_id: &mut usize,
-        jacobians: &mut DVector<Real>,
-        out: &mut Vec<Self>,
-        insert_at: Option<usize>,
-    ) {
-        let mut handle1 = joint.body1;
-        let mut handle2 = joint.body2;
-        let flipped = !bodies[handle2].is_dynamic();
-
-        let (local_frame1, local_frame2) = if flipped {
-            std::mem::swap(&mut handle1, &mut handle2);
-            (joint.data.local_frame2, joint.data.local_frame1)
-        } else {
-            (joint.data.local_frame1, joint.data.local_frame2)
-        };
-
-        let rb1 = &bodies[handle1];
-        let rb2 = &bodies[handle2];
-
-        let frame1 = rb1.pos.position * local_frame1;
-        let frame2 = rb2.pos.position * local_frame2;
-
-        let body1 = SolverBody {
-            linvel: rb1.vels.linvel,
-            angvel: rb1.vels.angvel,
-            im: rb1.mprops.effective_inv_mass,
-            sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
-            world_com: rb1.mprops.world_com,
-            mj_lambda: [crate::INVALID_USIZE],
-        };
-        let body2 = SolverBody {
-            linvel: rb2.vels.linvel,
-            angvel: rb2.vels.angvel,
-            im: rb2.mprops.effective_inv_mass,
-            sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
-            world_com: rb2.mprops.world_com,
-            mj_lambda: [rb2.ids.active_set_offset],
-        };
-
-        if let Some(mb2) = multibodies
-            .rigid_body_link(handle2)
-            .map(|link| (&multibodies[link.multibody], link.id))
-        {
-            let multibodies_ndof = mb2.0.ndofs();
-
-            if multibodies_ndof == 0 {
-                // The multibody is fixed, don’t generate any constraint.
-                return;
-            }
-
-            // For each solver contact we generate up to SPATIAL_DIM constraints, and each
-            // constraints appends the multibodies jacobian and weighted jacobians.
-            // Also note that for impulse_joints, the rigid-bodies will also add their jacobians
-            // to the generic DVector.
-            // TODO: is this count correct when we take both motors and limits into account?
-            let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
-
-            if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
-                jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
-            }
-
-            // TODO: find a way to avoid the temporary buffer.
-            let mut out_tmp = [JointGenericVelocityGroundConstraint::invalid(); 12];
-            let out_tmp_len = JointGenericVelocityGroundConstraint::lock_axes(
-                params,
-                joint_id,
-                &body1,
-                &body2,
-                mb2,
-                &frame1,
-                &frame2,
-                &joint.data,
-                jacobians,
-                j_id,
-                &mut out_tmp,
-            );
-
-            if let Some(at) = insert_at {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out[at + i] = AnyJointVelocityConstraint::JointGenericGroundConstraint(c);
-                }
-            } else {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
-                }
-            }
-        } else {
-            // TODO: find a way to avoid the temporary buffer.
-            let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12];
-            let out_tmp_len = JointVelocityGroundConstraint::<Real, 1>::lock_axes(
-                params,
-                joint_id,
-                &body1,
-                &body2,
-                &frame1,
-                &frame2,
-                &joint.data,
-                &mut out_tmp,
-            );
-
-            if let Some(at) = insert_at {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out[at + i] = AnyJointVelocityConstraint::JointGroundConstraint(c);
-                }
-            } else {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
-                }
-            }
-        }
-    }
-
-    #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint_ground(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
-        bodies: &RigidBodySet,
-        out: &mut Vec<Self>,
-        insert_at: Option<usize>,
-    ) {
-        use crate::dynamics::{
-            RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
-        };
-
-        let mut handles1 = gather![|ii| impulse_joints[ii].body1];
-        let mut handles2 = gather![|ii| impulse_joints[ii].body2];
-        let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].body_type];
-        let mut flipped = [false; SIMD_WIDTH];
-
-        for ii in 0..SIMD_WIDTH {
-            if !status2[ii].is_dynamic() {
-                std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
-                flipped[ii] = true;
-            }
-        }
-
-        let local_frame1: Isometry<SimdReal> = gather![|ii| if flipped[ii] {
-            impulse_joints[ii].data.local_frame2
-        } else {
-            impulse_joints[ii].data.local_frame1
-        }]
-        .into();
-        let local_frame2: Isometry<SimdReal> = gather![|ii| if flipped[ii] {
-            impulse_joints[ii].data.local_frame1
-        } else {
-            impulse_joints[ii].data.local_frame2
-        }]
-        .into();
-
-        let rbs1: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-        ) = (
-            gather![|ii| &bodies[handles1[ii]].pos],
-            gather![|ii| &bodies[handles1[ii]].vels],
-            gather![|ii| &bodies[handles1[ii]].mprops],
-        );
-        let rbs2: (
-            [&RigidBodyPosition; SIMD_WIDTH],
-            [&RigidBodyVelocity; SIMD_WIDTH],
-            [&RigidBodyMassProps; SIMD_WIDTH],
-            [&RigidBodyIds; SIMD_WIDTH],
-        ) = (
-            gather![|ii| &bodies[handles2[ii]].pos],
-            gather![|ii| &bodies[handles2[ii]].vels],
-            gather![|ii| &bodies[handles2[ii]].mprops],
-            gather![|ii| &bodies[handles2[ii]].ids],
-        );
-
-        let (rb_pos1, rb_vel1, rb_mprops1) = rbs1;
-        let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2;
-        let pos1: Isometry<SimdReal> = gather![|ii| rb_pos1[ii].position].into();
-        let pos2: Isometry<SimdReal> = gather![|ii| rb_pos2[ii].position].into();
-
-        let frame1 = pos1 * local_frame1;
-        let frame2 = pos2 * local_frame2;
-
-        let body1: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
-            linvel: gather![|ii| rb_vel1[ii].linvel].into(),
-            angvel: gather![|ii| rb_vel1[ii].angvel].into(),
-            im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(),
-            sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(),
-            world_com: gather![|ii| rb_mprops1[ii].world_com].into(),
-            mj_lambda: [crate::INVALID_USIZE; SIMD_WIDTH],
-        };
-        let body2: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
-            linvel: gather![|ii| rb_vel2[ii].linvel].into(),
-            angvel: gather![|ii| rb_vel2[ii].angvel].into(),
-            im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(),
-            sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(),
-            world_com: gather![|ii| rb_mprops2[ii].world_com].into(),
-            mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset],
-        };
-
-        // TODO: find a way to avoid the temporary buffer.
-        let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12];
-        let out_tmp_len = JointVelocityGroundConstraint::<SimdReal, SIMD_WIDTH>::lock_axes(
-            params,
-            joint_id,
-            &body1,
-            &body2,
-            &frame1,
-            &frame2,
-            impulse_joints[0].data.locked_axes.bits(),
-            &mut out_tmp,
-        );
-
-        if let Some(at) = insert_at {
-            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                out[at + i] = AnyJointVelocityConstraint::JointGroundConstraintSimd(c);
-            }
-        } else {
-            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
-            }
-        }
-    }
-
-    pub fn remove_bias_from_rhs(&mut self) {
-        match self {
-            AnyJointVelocityConstraint::JointConstraint(c) => c.remove_bias_from_rhs(),
-            AnyJointVelocityConstraint::JointGroundConstraint(c) => c.remove_bias_from_rhs(),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointConstraintSimd(c) => c.remove_bias_from_rhs(),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.remove_bias_from_rhs(),
-            AnyJointVelocityConstraint::JointGenericConstraint(c) => c.remove_bias_from_rhs(),
-            AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => c.remove_bias_from_rhs(),
-            AnyJointVelocityConstraint::Empty => unreachable!(),
-        }
-    }
-
-    pub fn solve(
-        &mut self,
-        jacobians: &DVector<Real>,
-        mj_lambdas: &mut [DeltaVel<Real>],
-        generic_mj_lambdas: &mut DVector<Real>,
-    ) {
-        match self {
-            AnyJointVelocityConstraint::JointConstraint(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::JointGroundConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointConstraintSimd(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::JointGenericConstraint(c) => {
-                c.solve(jacobians, mj_lambdas, generic_mj_lambdas)
-            }
-            AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => {
-                c.solve(jacobians, mj_lambdas, generic_mj_lambdas)
-            }
-            AnyJointVelocityConstraint::Empty => unreachable!(),
-        }
-    }
-
-    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
-        match self {
-            AnyJointVelocityConstraint::JointConstraint(c) => c.writeback_impulses(joints_all),
-            AnyJointVelocityConstraint::JointGroundConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointConstraintSimd(c) => c.writeback_impulses(joints_all),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            AnyJointVelocityConstraint::JointGenericConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            AnyJointVelocityConstraint::Empty => unreachable!(),
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
index f7101a1..00bead1 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
@@ -1,19 +1,349 @@
 use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
-    JointVelocityConstraint, JointVelocityGroundConstraint, WritebackId,
+    JointFixedSolverBody, JointOneBodyConstraint, JointTwoBodyConstraint, WritebackId,
 };
-use crate::dynamics::solver::joint_constraint::SolverBody;
+use crate::dynamics::solver::joint_constraint::JointSolverBody;
+use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::ConstraintsCounts;
 use crate::dynamics::solver::MotorParameters;
-use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits};
+use crate::dynamics::{GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex};
 use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
+use crate::prelude::RigidBodySet;
 use crate::utils;
-use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
+use crate::utils::{IndexMut2, SimdCrossMatrix, SimdDot, SimdQuat, SimdRealCopy};
 use na::SMatrix;
 
 #[cfg(feature = "dim3")]
-use crate::utils::WBasis;
+use crate::utils::SimdBasis;
+
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::{SimdReal, SIMD_WIDTH};
+
+pub struct JointTwoBodyConstraintBuilder {
+    body1: usize,
+    body2: usize,
+    joint_id: JointIndex,
+    joint: GenericJoint,
+    constraint_id: usize,
+}
+
+impl JointTwoBodyConstraintBuilder {
+    pub fn generate(
+        joint: &ImpulseJoint,
+        bodies: &RigidBodySet,
+        joint_id: JointIndex,
+        out_builder: &mut Self,
+        out_constraint_id: &mut usize,
+    ) {
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
+        *out_builder = Self {
+            body1: rb1.ids.active_set_offset,
+            body2: rb2.ids.active_set_offset,
+            joint_id,
+            joint: joint.data,
+            constraint_id: *out_constraint_id,
+        };
+
+        let count = ConstraintsCounts::from_joint(joint);
+        *out_constraint_id += count.num_constraints;
+    }
+
+    pub fn update(
+        &self,
+        params: &IntegrationParameters,
+        bodies: &[SolverBody],
+        out: &mut [JointTwoBodyConstraint<Real, 1>],
+    ) {
+        // NOTE: right now, the "update", is basically reconstructing all the
+        //       constraints. Could we make this more incremental?
+
+        let rb1 = &bodies[self.body1];
+        let rb2 = &bodies[self.body2];
+        let frame1 = rb1.position * self.joint.local_frame1;
+        let frame2 = rb2.position * self.joint.local_frame2;
+
+        let joint_body1 = JointSolverBody {
+            im: rb1.im,
+            sqrt_ii: rb1.sqrt_ii,
+            world_com: rb1.world_com,
+            solver_vel: [self.body1],
+        };
+        let joint_body2 = JointSolverBody {
+            im: rb2.im,
+            sqrt_ii: rb2.sqrt_ii,
+            world_com: rb2.world_com,
+            solver_vel: [self.body2],
+        };
+
+        JointTwoBodyConstraint::<Real, 1>::lock_axes(
+            params,
+            self.joint_id,
+            &joint_body1,
+            &joint_body2,
+            &frame1,
+            &frame2,
+            &self.joint,
+            &mut out[self.constraint_id..],
+        );
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+pub struct JointTwoBodyConstraintBuilderSimd {
+    body1: [usize; SIMD_WIDTH],
+    body2: [usize; SIMD_WIDTH],
+    joint_body1: JointSolverBody<SimdReal, SIMD_WIDTH>,
+    joint_body2: JointSolverBody<SimdReal, SIMD_WIDTH>,
+    joint_id: [JointIndex; SIMD_WIDTH],
+    local_frame1: Isometry<SimdReal>,
+    local_frame2: Isometry<SimdReal>,
+    locked_axes: u8,
+    constraint_id: usize,
+}
+
+#[cfg(feature = "simd-is-enabled")]
+impl JointTwoBodyConstraintBuilderSimd {
+    pub fn generate(
+        joint: [&ImpulseJoint; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        out_builder: &mut Self,
+        out_constraint_id: &mut usize,
+    ) {
+        let rb1 = gather![|ii| &bodies[joint[ii].body1]];
+        let rb2 = gather![|ii| &bodies[joint[ii].body2]];
+
+        let body1 = gather![|ii| rb1[ii].ids.active_set_offset];
+        let body2 = gather![|ii| rb2[ii].ids.active_set_offset];
+
+        let joint_body1 = JointSolverBody {
+            im: gather![|ii| rb1[ii].mprops.effective_inv_mass].into(),
+            sqrt_ii: gather![|ii| rb1[ii].mprops.effective_world_inv_inertia_sqrt].into(),
+            world_com: Point::origin(),
+            solver_vel: body1,
+        };
+        let joint_body2 = JointSolverBody {
+            im: gather![|ii| rb2[ii].mprops.effective_inv_mass].into(),
+            sqrt_ii: gather![|ii| rb2[ii].mprops.effective_world_inv_inertia_sqrt].into(),
+            world_com: Point::origin(),
+            solver_vel: body2,
+        };
+
+        *out_builder = Self {
+            body1,
+            body2,
+            joint_body1,
+            joint_body2,
+            joint_id,
+            local_frame1: gather![|ii| joint[ii].data.local_frame1].into(),
+            local_frame2: gather![|ii| joint[ii].data.local_frame2].into(),
+            locked_axes: joint[0].data.locked_axes.bits(),
+            constraint_id: *out_constraint_id,
+        };
+
+        let count = ConstraintsCounts::from_joint(joint[0]);
+        *out_constraint_id += count.num_constraints;
+    }
+
+    pub fn update(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &[SolverBody],
+        out: &mut [JointTwoBodyConstraint<SimdReal, SIMD_WIDTH>],
+    ) {
+        // NOTE: right now, the "update", is basically reconstructing all the
+        //       constraints. Could we make this more incremental?
+
+        let rb1 = gather![|ii| &bodies[self.body1[ii]]];
+        let rb2 = gather![|ii| &bodies[self.body2[ii]]];
+        let frame1 = Isometry::from(gather![|ii| rb1[ii].position]) * self.local_frame1;
+        let frame2 =