diff --git a/.github/workflows/rapier-ci-build.yml b/.github/workflows/rapier-ci-build.yml
index 08c5da5..b4bc538 100644
--- a/.github/workflows/rapier-ci-build.yml
+++ b/.github/workflows/rapier-ci-build.yml
@@ -28,13 +28,13 @@ jobs:
     - name: Build rapier3d
       run: cargo build --verbose -p rapier3d;
     - name: Build rapier2d SIMD
-      run: cd build/rapier2d; cargo build --verbose --features simd-stable;
+      run: cd crates/rapier2d; cargo build --verbose --features simd-stable;
     - name: Build rapier3d SIMD
-      run: cd build/rapier3d; cargo build --verbose --features simd-stable;
+      run: cd crates/rapier3d; cargo build --verbose --features simd-stable;
     - name: Build rapier2d SIMD Parallel
-      run: cd build/rapier2d; cargo build --verbose --features simd-stable --features parallel;
+      run: cd crates/rapier2d; cargo build --verbose --features simd-stable --features parallel;
     - name: Build rapier3d SIMD Parallel
-      run: cd build/rapier3d; cargo build --verbose --features simd-stable --features parallel;
+      run: cd crates/rapier3d; cargo build --verbose --features simd-stable --features parallel;
     - name: Run tests
       run: cargo test
     - name: Check rapier_testbed2d
@@ -42,9 +42,9 @@ jobs:
     - name: Check rapier_testbed3d
       run: cargo check --verbose -p rapier_testbed3d;
     - name: Check rapier_testbed2d --features parallel
-      run: cd build/rapier_testbed2d; cargo check --verbose --features parallel;
+      run: cd crates/rapier_testbed2d; cargo check --verbose --features parallel;
     - name: Check rapier_testbed3d --features parallel
-      run: cd build/rapier_testbed3d; cargo check --verbose --features parallel;
+      run: cd crates/rapier_testbed3d; cargo check --verbose --features parallel;
     - name: Check rapier-examples-2d
       run: cargo check -j 1 --verbose -p rapier-examples-2d;
     - name: Check rapier-examples-3d
@@ -57,9 +57,9 @@ jobs:
       - uses: actions/checkout@v2
       - run: rustup target add wasm32-unknown-unknown
       - name: build rapier2d
-        run: cd build/rapier2d && cargo build --verbose --features wasm-bindgen --target wasm32-unknown-unknown;
+        run: cd crates/rapier2d && cargo build --verbose --features wasm-bindgen --target wasm32-unknown-unknown;
       - name: build rapier3d
-        run: cd build/rapier3d && cargo build --verbose --features wasm-bindgen --target wasm32-unknown-unknown;
+        run: cd crates/rapier3d && cargo build --verbose --features wasm-bindgen --target wasm32-unknown-unknown;
   build-wasm-emscripten:
     runs-on: ubuntu-latest
     env:
@@ -68,10 +68,10 @@ jobs:
       - uses: actions/checkout@v2
       - run: rustup target add wasm32-unknown-emscripten
       - name: build rapier2d
-        run: cd build/rapier2d && cargo build --verbose --target wasm32-unknown-emscripten;
+        run: cd crates/rapier2d && cargo build --verbose --target wasm32-unknown-emscripten;
       - name: build rapier3d
-        run: cd build/rapier3d && cargo build --verbose --target wasm32-unknown-emscripten;
+        run: cd crates/rapier3d && cargo build --verbose --target wasm32-unknown-emscripten;
       - name: build rapier2d --features simd-stable
-        run: cd build/rapier2d && cargo build --verbose --target wasm32-unknown-emscripten --features simd-stable;
+        run: cd crates/rapier2d && cargo build --verbose --target wasm32-unknown-emscripten --features simd-stable;
       - name: build rapier3d --features simd-stable
-        run: cd build/rapier3d && cargo build --verbose --target wasm32-unknown-emscripten --features simd-stable;
+        run: cd crates/rapier3d && cargo build --verbose --target wasm32-unknown-emscripten --features simd-stable;
diff --git a/Cargo.toml b/Cargo.toml
index 7021ade..1bd8c67 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,21 +1,17 @@
 [workspace]
-members = [ "build/rapier2d", "build/rapier2d-f64", "build/rapier_testbed2d", "examples2d", "benchmarks2d",
-            "build/rapier3d", "build/rapier3d-f64", "build/rapier_testbed3d", "examples3d", "benchmarks3d" ]
+members = [ "crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "examples2d", "benchmarks2d",
+            "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "examples3d", "benchmarks3d" ]
 resolver = "2"
 
 [patch.crates-io]
 #wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" }
 
 #simba = { path = "../simba" }
-#ncollide2d = { path = "../ncollide/build/ncollide2d" }
-#ncollide3d = { path = "../ncollide/build/ncollide3d" }
-#nphysics2d = { path = "../nphysics/build/nphysics2d" }
-#nphysics3d = { path = "../nphysics/build/nphysics3d" }
 #kiss3d = { path = "../kiss3d" }
-#parry2d = { path = "../parry/build/parry2d" }
-#parry3d = { path = "../parry/build/parry3d" }
-#parry2d-f64 = { path = "../parry/build/parry2d-f64" }
-#parry3d-f64 = { path = "../parry/build/parry3d-f64" }
+#parry2d = { path = "../parry/crates/parry2d" }
+#parry3d = { path = "../parry/crates/parry3d" }
+#parry2d-f64 = { path = "../parry/crates/parry2d-f64" }
+#parry3d-f64 = { path = "../parry/crates/parry3d-f64" }
 #nalgebra = { path = "../nalgebra" }
 
 #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" }
@@ -24,14 +20,10 @@ resolver = "2"
 #parry3d = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
 #parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
 #parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
-#ncollide2d = { git = "https://github.com/dimforge/ncollide" }
-#ncollide3d = { git = "https://github.com/dimforge/ncollide" }
-#nphysics2d = { git = "https://github.com/dimforge/nphysics" }
-#nphysics3d = { git = "https://github.com/dimforge/nphysics" }
 
 [profile.release]
 #debug = true
-codegen-units = 1
+#codegen-units = 1
 #opt-level = 1
 #lto = true
 
diff --git a/benchmarks2d/Cargo.toml b/benchmarks2d/Cargo.toml
index c7c775d..bbab4d9 100644
--- a/benchmarks2d/Cargo.toml
+++ b/benchmarks2d/Cargo.toml
@@ -2,7 +2,7 @@
 name    = "rapier-benchmarks-2d"
 version = "0.1.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
-edition = "2018"
+edition = "2021"
 
 [features]
 parallel = [ "rapier2d/parallel", "rapier_testbed2d/parallel" ]
@@ -16,10 +16,10 @@ rand       = "0.8"
 Inflector  = "0.11"
 
 [dependencies.rapier_testbed2d]
-path = "../build/rapier_testbed2d"
+path = "../crates/rapier_testbed2d"
 
 [dependencies.rapier2d]
-path = "../build/rapier2d"
+path = "../crates/rapier2d"
 
 [[bin]]
 name = "all_benchmarks2"
diff --git a/benchmarks2d/balls2.rs b/benchmarks2d/balls2.rs
index ec55f24..168acaf 100644
--- a/benchmarks2d/balls2.rs
+++ b/benchmarks2d/balls2.rs
@@ -7,7 +7,9 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
     /*
      * Ground
      */
@@ -58,6 +60,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 2.5], 5.0);
 }
diff --git a/benchmarks2d/boxes2.rs b/benchmarks2d/boxes2.rs
index 2e4c5e4..c3d7445 100644
--- a/benchmarks2d/boxes2.rs
+++ b/benchmarks2d/boxes2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -63,6 +64,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 50.0], 10.0);
 }
diff --git a/benchmarks2d/capsules2.rs b/benchmarks2d/capsules2.rs
index e75afe4..3bfa6ab 100644
--- a/benchmarks2d/capsules2.rs
+++ b/benchmarks2d/capsules2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -65,6 +66,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 50.0], 10.0);
 }
diff --git a/benchmarks2d/convex_polygons2.rs b/benchmarks2d/convex_polygons2.rs
index 6c9792e..6678460 100644
--- a/benchmarks2d/convex_polygons2.rs
+++ b/benchmarks2d/convex_polygons2.rs
@@ -9,7 +9,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -76,6 +77,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 50.0], 10.0);
 }
diff --git a/benchmarks2d/heightfield2.rs b/benchmarks2d/heightfield2.rs
index 3178e60..04c56da 100644
--- a/benchmarks2d/heightfield2.rs
+++ b/benchmarks2d/heightfield2.rs
@@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -63,6 +64,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 50.0], 10.0);
 }
diff --git a/benchmarks2d/joint_ball2.rs b/benchmarks2d/joint_ball2.rs
index 114fe85..e856556 100644
--- a/benchmarks2d/joint_ball2.rs
+++ b/benchmarks2d/joint_ball2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Create the balls
@@ -41,16 +42,16 @@ pub fn init_world(testbed: &mut Testbed) {
             // Vertical joint.
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
-                let joint = BallJoint::new(Point::origin(), point![0.0, shift]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = RevoluteJoint::new().local_anchor2(point![0.0, shift]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             // Horizontal joint.
             if k > 0 {
                 let parent_index = body_handles.len() - numi;
                 let parent_handle = body_handles[parent_index];
-                let joint = BallJoint::new(Point::origin(), point![-shift, 0.0]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = RevoluteJoint::new().local_anchor2(point![-shift, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             body_handles.push(child_handle);
@@ -60,6 +61,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![numk as f32 * rad, numi as f32 * -rad], 5.0);
 }
diff --git a/benchmarks2d/joint_fixed2.rs b/benchmarks2d/joint_fixed2.rs
index 8d6e8dc..690b8cb 100644
--- a/benchmarks2d/joint_fixed2.rs
+++ b/benchmarks2d/joint_fixed2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Create the balls
@@ -46,22 +47,18 @@ pub fn init_world(testbed: &mut Testbed) {
                     // Vertical joint.
                     if i > 0 {
                         let parent_handle = *body_handles.last().unwrap();
-                        let joint = FixedJoint::new(
-                            Isometry::identity(),
-                            Isometry::translation(0.0, shift),
-                        );
-                        joints.insert(parent_handle, child_handle, joint);
+                        let joint =
+                            FixedJoint::new().local_frame2(Isometry::translation(0.0, shift));
+                        impulse_joints.insert(parent_handle, child_handle, joint);
                     }
 
                     // Horizontal joint.
                     if k > 0 {
                         let parent_index = body_handles.len() - num;
                         let parent_handle = body_handles[parent_index];
-                        let joint = FixedJoint::new(
-                            Isometry::identity(),
-                            Isometry::translation(-shift, 0.0),
-                        );
-                        joints.insert(parent_handle, child_handle, joint);
+                        let joint =
+                            FixedJoint::new().local_frame2(Isometry::translation(-shift, 0.0));
+                        impulse_joints.insert(parent_handle, child_handle, joint);
                     }
 
                     body_handles.push(child_handle);
@@ -73,6 +70,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![50.0, 50.0], 5.0);
 }
diff --git a/benchmarks2d/joint_prismatic2.rs b/benchmarks2d/joint_prismatic2.rs
index c2b4bf3..4733088 100644
--- a/benchmarks2d/joint_prismatic2.rs
+++ b/benchmarks2d/joint_prismatic2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Create the balls
@@ -46,12 +47,10 @@ pub fn init_world(testbed: &mut Testbed) {
                     UnitVector::new_normalize(vector![-1.0, 1.0])
                 };
 
-                let mut prism =
-                    PrismaticJoint::new(Point::origin(), axis, point![0.0, shift], axis);
-                prism.limits_enabled = true;
-                prism.limits[0] = -1.5;
-                prism.limits[1] = 1.5;
-                joints.insert(curr_parent, curr_child, prism);
+                let mut prism = PrismaticJoint::new(axis)
+                    .local_anchor2(point![0.0, shift])
+                    .limit_axis([-1.5, 1.5]);
+                impulse_joints.insert(curr_parent, curr_child, prism);
 
                 curr_parent = curr_child;
             }
@@ -61,6 +60,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![80.0, 80.0], 15.0);
 }
diff --git a/benchmarks2d/pyramid2.rs b/benchmarks2d/pyramid2.rs
index a557ff4..61636b9 100644
--- a/benchmarks2d/pyramid2.rs
+++ b/benchmarks2d/pyramid2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -50,6 +51,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 2.5], 5.0);
 }
diff --git a/benchmarks3d/Cargo.toml b/benchmarks3d/Cargo.toml
index 9b76df3..76d1911 100644
--- a/benchmarks3d/Cargo.toml
+++ b/benchmarks3d/Cargo.toml
@@ -2,7 +2,7 @@
 name    = "rapier-benchmarks-3d"
 version = "0.1.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
-edition = "2018"
+edition = "2021"
 
 [features]
 parallel = [ "rapier3d/parallel", "rapier_testbed3d/parallel" ]
@@ -16,10 +16,10 @@ rand       = "0.8"
 Inflector  = "0.11"
 
 [dependencies.rapier_testbed3d]
-path = "../build/rapier_testbed3d"
+path = "../crates/rapier_testbed3d"
 
 [dependencies.rapier3d]
-path = "../build/rapier3d"
+path = "../crates/rapier3d"
 
 [[bin]]
 name = "all_benchmarks3"
diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs
index f61b80f..6a3f756 100644
--- a/benchmarks3d/all_benchmarks3.rs
+++ b/benchmarks3d/all_benchmarks3.rs
@@ -58,10 +58,10 @@ pub fn main() {
         ("Stacks", stacks3::init_world),
         ("Pyramid", pyramid3::init_world),
         ("Trimesh", trimesh3::init_world),
-        ("Joint ball", joint_ball3::init_world),
-        ("Joint fixed", joint_fixed3::init_world),
-        ("Joint revolute", joint_revolute3::init_world),
-        ("Joint prismatic", joint_prismatic3::init_world),
+        ("ImpulseJoint ball", joint_ball3::init_world),
+        ("ImpulseJoint fixed", joint_fixed3::init_world),
+        ("ImpulseJoint revolute", joint_revolute3::init_world),
+        ("ImpulseJoint prismatic", joint_prismatic3::init_world),
         ("Keva tower", keva3::init_world),
     ];
 
diff --git a/benchmarks3d/balls3.rs b/benchmarks3d/balls3.rs
index 3f0bf36..b4d5102 100644
--- a/benchmarks3d/balls3.rs
+++ b/benchmarks3d/balls3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Create the balls
@@ -48,6 +49,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/boxes3.rs b/benchmarks3d/boxes3.rs
index 9c7ed81..0250619 100644
--- a/benchmarks3d/boxes3.rs
+++ b/benchmarks3d/boxes3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -58,6 +59,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/capsules3.rs b/benchmarks3d/capsules3.rs
index 8565503..5b05f23 100644
--- a/benchmarks3d/capsules3.rs
+++ b/benchmarks3d/capsules3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -59,6 +60,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/ccd3.rs b/benchmarks3d/ccd3.rs
index 987aba6..d54ebc1 100644
--- a/benchmarks3d/ccd3.rs
+++ b/benchmarks3d/ccd3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/compound3.rs b/benchmarks3d/compound3.rs
index a914ce9..872923c 100644
--- a/benchmarks3d/compound3.rs
+++ b/benchmarks3d/compound3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -66,6 +67,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/convex_polyhedron3.rs b/benchmarks3d/convex_polyhedron3.rs
index 7065cd5..cb834ea 100644
--- a/benchmarks3d/convex_polyhedron3.rs
+++ b/benchmarks3d/convex_polyhedron3.rs
@@ -9,7 +9,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/heightfield3.rs b/benchmarks3d/heightfield3.rs
index 32856fe..b95f1ee 100644
--- a/benchmarks3d/heightfield3.rs
+++ b/benchmarks3d/heightfield3.rs
@@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/joint_ball3.rs b/benchmarks3d/joint_ball3.rs
index a1661f8..64128ba 100644
--- a/benchmarks3d/joint_ball3.rs
+++ b/benchmarks3d/joint_ball3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     let rad = 0.4;
     let num = 100;
@@ -36,16 +37,16 @@ pub fn init_world(testbed: &mut Testbed) {
             // Vertical joint.
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
-                let joint = BallJoint::new(Point::origin(), point![0.0, 0.0, -shift]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             // Horizontal joint.
             if k > 0 {
                 let parent_index = body_handles.len() - num;
                 let parent_handle = body_handles[parent_index];
-                let joint = BallJoint::new(Point::origin(), point![-shift, 0.0, 0.0]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             body_handles.push(child_handle);
@@ -55,6 +56,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![-110.0, -46.0, 170.0], point![54.0, -38.0, 29.0]);
 }
diff --git a/benchmarks3d/joint_fixed3.rs b/benchmarks3d/joint_fixed3.rs
index 3d1e317..b3f4039 100644
--- a/benchmarks3d/joint_fixed3.rs
+++ b/benchmarks3d/joint_fixed3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     let rad = 0.4;
     let num = 5;
@@ -49,22 +50,16 @@ pub fn init_world(testbed: &mut Testbed) {
                         // Vertical joint.
                         if i > 0 {
                             let parent_handle = *body_handles.last().unwrap();
-                            let joint = FixedJoint::new(
-                                Isometry::identity(),
-                                Isometry::translation(0.0, 0.0, -shift),
-                            );
-                            joints.insert(parent_handle, child_handle, joint);
+                            let joint = FixedJoint::new().local_anchor2(point![0.0, 0.0, -shift]);
+                            impulse_joints.insert(parent_handle, child_handle, joint);
                         }
 
                         // Horizontal joint.
                         if k > 0 {
                             let parent_index = body_handles.len() - num;
                             let parent_handle = body_handles[parent_index];
-                            let joint = FixedJoint::new(
-                                Isometry::identity(),
-                                Isometry::translation(-shift, 0.0, 0.0),
-                            );
-                            joints.insert(parent_handle, child_handle, joint);
+                            let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
+                            impulse_joints.insert(parent_handle, child_handle, joint);
                         }
 
                         body_handles.push(child_handle);
@@ -77,6 +72,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![-38.0, 14.0, 108.0], point![46.0, 12.0, 23.0]);
 }
diff --git a/benchmarks3d/joint_prismatic3.rs b/benchmarks3d/joint_prismatic3.rs
index b011662..80839d7 100644
--- a/benchmarks3d/joint_prismatic3.rs
+++ b/benchmarks3d/joint_prismatic3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     let rad = 0.4;
     let num = 5;
@@ -47,19 +48,10 @@ pub fn init_world(testbed: &mut Testbed) {
                         UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
                     };
 
-                    let z = Vector::z();
-                    let mut prism = PrismaticJoint::new(
-                        Point::origin(),
-                        axis,
-                        z,
-                        point![0.0, 0.0, -shift],
-                        axis,
-                        z,
-                    );
-                    prism.limits_enabled = true;
-                    prism.limits[0] = -2.0;
-                    prism.limits[1] = 2.0;
-                    joints.insert(curr_parent, curr_child, prism);
+                    let prism = PrismaticJoint::new(axis)
+                        .local_anchor2(point![0.0, 0.0, -shift])
+                        .limit_axis([-2.0, 0.0]);
+                    impulse_joints.insert(curr_parent, curr_child, prism);
 
                     curr_parent = curr_child;
                 }
@@ -70,6 +62,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![262.0, 63.0, 124.0], point![101.0, 4.0, -3.0]);
 }
diff --git a/benchmarks3d/joint_revolute3.rs b/benchmarks3d/joint_revolute3.rs
index d6dc06c..8bdf0e9 100644
--- a/benchmarks3d/joint_revolute3.rs
+++ b/benchmarks3d/joint_revolute3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     let rad = 0.4;
     let num = 10;
@@ -49,22 +50,21 @@ pub fn init_world(testbed: &mut Testbed) {
                     colliders.insert_with_parent(collider, handles[k], &mut bodies);
                 }
 
-                // Setup four joints.
-                let o = Point::origin();
+                // Setup four impulse_joints.
                 let x = Vector::x_axis();
                 let z = Vector::z_axis();
 
                 let revs = [
-                    RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
-                    RevoluteJoint::new(o, x, point![-shift, 0.0, 0.0], x),
-                    RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
-                    RevoluteJoint::new(o, x, point![shift, 0.0, 0.0], x),
+                    RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]),
+                    RevoluteJoint::new(x).local_anchor2(point![-shift, 0.0, 0.0]),
+                    RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]),
+                    RevoluteJoint::new(x).local_anchor2(point![shift, 0.0, 0.0]),
                 ];
 
-                joints.insert(curr_parent, handles[0], revs[0]);
-                joints.insert(handles[0], handles[1], revs[1]);
-                joints.insert(handles[1], handles[2], revs[2]);
-                joints.insert(handles[2], handles[3], revs[3]);
+                impulse_joints.insert(curr_parent, handles[0], revs[0]);
+                impulse_joints.insert(handles[0], handles[1], revs[1]);
+                impulse_joints.insert(handles[1], handles[2], revs[2]);
+                impulse_joints.insert(handles[2], handles[3], revs[3]);
 
                 curr_parent = handles[3];
             }
@@ -74,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![478.0, 83.0, 228.0], point![134.0, 83.0, -116.0]);
 }
diff --git a/benchmarks3d/keva3.rs b/benchmarks3d/keva3.rs
index 38a0432..ad9e1ae 100644
--- a/benchmarks3d/keva3.rs
+++ b/benchmarks3d/keva3.rs
@@ -83,7 +83,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -132,6 +133,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/pyramid3.rs b/benchmarks3d/pyramid3.rs
index 655ecb7..b378da4 100644
--- a/benchmarks3d/pyramid3.rs
+++ b/benchmarks3d/pyramid3.rs
@@ -41,7 +41,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/stacks3.rs b/benchmarks3d/stacks3.rs
index 39386ea..7fc9097 100644
--- a/benchmarks3d/stacks3.rs
+++ b/benchmarks3d/stacks3.rs
@@ -101,7 +101,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -183,6 +184,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/trimesh3.rs b/benchmarks3d/trimesh3.rs
index 8621f19..1039a09 100644
--- a/benchmarks3d/trimesh3.rs
+++ b/benchmarks3d/trimesh3.rs
@@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -78,6 +79,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/build/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml
similarity index 93%
rename from build/rapier2d-f64/Cargo.toml
rename to crates/rapier2d-f64/Cargo.toml
index 5d85639..525e620 100644
--- a/build/rapier2d-f64/Cargo.toml
+++ b/crates/rapier2d-f64/Cargo.toml
@@ -8,9 +8,9 @@ homepage = "http://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
 categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
 license = "Apache-2.0"
-edition = "2018"
+edition = "2021"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -47,9 +47,9 @@ required-features = [ "dim2", "f64" ]
 vec_map = { version = "0.8", optional = true }
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
-nalgebra = "0.29"
-parry2d-f64 = "^0.7.1"
-simba = "0.6"
+nalgebra = "0.30"
+parry2d-f64 = "0.8"
+simba = "0.7"
 approx = "0.5"
 rayon = { version = "1", optional = true }
 crossbeam = "0.8"
diff --git a/build/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml
similarity index 93%
rename from build/rapier2d/Cargo.toml
rename to crates/rapier2d/Cargo.toml
index 9bc5f3b..64985d6 100644
--- a/build/rapier2d/Cargo.toml
+++ b/crates/rapier2d/Cargo.toml
@@ -8,9 +8,9 @@ homepage = "http://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
 categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
 license = "Apache-2.0"
-edition = "2018"
+edition = "2021"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -47,9 +47,9 @@ required-features = [ "dim2", "f32" ]
 vec_map = { version = "0.8", optional = true }
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
-nalgebra = "0.29"
-parry2d = "^0.7.1"
-simba = "0.6"
+nalgebra = "0.30"
+parry2d = "0.8"
+simba = "0.7"
 approx = "0.5"
 rayon = { version = "1", optional = true }
 crossbeam = "0.8"
diff --git a/build/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml
similarity index 92%
rename from build/rapier3d-f64/Cargo.toml
rename to crates/rapier3d-f64/Cargo.toml
index a6cdd00..77cf8d4 100644
--- a/build/rapier3d-f64/Cargo.toml
+++ b/crates/rapier3d-f64/Cargo.toml
@@ -8,9 +8,9 @@ homepage = "http://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
 categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
 license = "Apache-2.0"
-edition = "2018"
+edition = "2021"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -47,9 +47,9 @@ required-features = [ "dim3", "f64" ]
 vec_map = { version = "0.8", optional = true }
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
-nalgebra = "0.29"
-parry3d-f64 = "^0.7.1"
-simba = "0.6"
+nalgebra = "0.30"
+parry3d-f64 = "0.8"
+simba = "0.7"
 approx = "0.5"
 rayon = { version = "1", optional = true }
 crossbeam = "0.8"
diff --git a/build/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml
similarity index 93%
rename from build/rapier3d/Cargo.toml
rename to crates/rapier3d/Cargo.toml
index 2830c07..75ca2ba 100644
--- a/build/rapier3d/Cargo.toml
+++ b/crates/rapier3d/Cargo.toml
@@ -8,9 +8,9 @@ homepage = "http://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
 categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
 license = "Apache-2.0"
-edition = "2018"
+edition = "2021"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -47,9 +47,9 @@ required-features = [ "dim3", "f32" ]
 vec_map = { version = "0.8", optional = true }
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
-nalgebra = "0.29"
-parry3d = "^0.7.1"
-simba = "0.6"
+nalgebra = "0.30"
+parry3d = "0.8"
+simba = "0.7"
 approx = "0.5"
 rayon = { version = "1", optional = true }
 crossbeam = "0.8"
diff --git a/build/rapier_testbed2d/Cargo.toml b/crates/rapier_testbed2d/Cargo.toml
similarity index 81%
rename from build/rapier_testbed2d/Cargo.toml
rename to crates/rapier_testbed2d/Cargo.toml
index 0ba4803..ea5db88 100644
--- a/build/rapier_testbed2d/Cargo.toml
+++ b/crates/rapier_testbed2d/Cargo.toml
@@ -6,9 +6,9 @@ description = "Testbed for the Rapier 2-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
 repository = "https://github.com/dimforge/rapier"
 categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
 license = "Apache-2.0"
-edition = "2018"
+edition = "2021"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -22,20 +22,18 @@ required-features = [ "dim2" ]
 default = [ "dim2" ]
 dim2 = [ ]
 parallel = [ "rapier2d/parallel", "num_cpus" ]
-other-backends = [ "wrapped2d", "nphysics2d", "ncollide2d" ]
+other-backends = [ "wrapped2d" ]
 
 
 [dependencies]
-nalgebra   = { version = "0.29", features = [ "rand" ] }
+nalgebra   = { version = "0.30", features = [ "rand" ] }
 rand       = "0.8"
 rand_pcg   = "0.3"
 instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 num_cpus   = { version = "1", optional = true }
 wrapped2d  = { version = "0.4", optional = true }
-parry2d = "0.7"
-ncollide2d = { version = "0.32", optional = true }
-nphysics2d = { version = "0.24", optional = true }
+parry2d = "0.8"
 crossbeam = "0.8"
 bincode = "1"
 Inflector  = "0.11"
diff --git a/build/rapier_testbed3d/Cargo.toml b/crates/rapier_testbed3d/Cargo.toml
similarity index 82%
rename from build/rapier_testbed3d/Cargo.toml
rename to crates/rapier_testbed3d/Cargo.toml
index 1ec0084..8a8a1e4 100644
--- a/build/rapier_testbed3d/Cargo.toml
+++ b/crates/rapier_testbed3d/Cargo.toml
@@ -6,9 +6,9 @@ description = "Testbed for the Rapier 3-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
 repository = "https://github.com/dimforge/rapier"
 categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
 license = "Apache-2.0"
-edition = "2018"
+edition = "2021"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -22,19 +22,17 @@ required-features = [ "dim3" ]
 default = [ "dim3" ]
 dim3 = [ ]
 parallel = [ "rapier3d/parallel", "num_cpus" ]
-other-backends = [ "physx", "physx-sys", "glam", "nphysics3d", "ncollide3d" ]
+other-backends = [ "physx", "physx-sys", "glam" ]
 
 [dependencies]
-nalgebra   = { version = "0.29", features = [ "rand" ] }
+nalgebra   = { version = "0.30", features = [ "rand" ] }
 rand       = "0.8"
 rand_pcg   = "0.3"
 instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 glam       = { version = "0.12", optional = true }
 num_cpus   = { version = "1", optional = true }
-parry3d    = "0.7"
-ncollide3d = { version = "0.32", optional = true }
-nphysics3d = { version = "0.24", optional = true }
+parry3d    = "0.8"
 physx      = { version = "0.11", optional = true }
 physx-sys = { version = "0.4", optional = true }
 crossbeam = "0.8"
diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml
index 3c065fd..415f994 100644
--- a/examples2d/Cargo.toml
+++ b/examples2d/Cargo.toml
@@ -2,7 +2,7 @@
 name    = "rapier-examples-2d"
 version = "0.1.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
-edition = "2018"
+edition = "2021"
 default-run = "all_examples2"
 
 [features]
@@ -19,10 +19,10 @@ lyon       = "0.17"
 usvg       = "0.14"
 
 [dependencies.rapier_testbed2d]
-path = "../build/rapier_testbed2d"
+path = "../crates/rapier_testbed2d"
 
 [dependencies.rapier2d]
-path = "../build/rapier2d"
+path = "../crates/rapier2d"
 
 [[bin]]
 name = "all_examples2"
diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs
index f701a90..0919da9 100644
--- a/examples2d/add_remove2.rs
+++ b/examples2d/add_remove2.rs
@@ -4,12 +4,15 @@ use rapier_testbed2d::Testbed;
 pub fn init_world(testbed: &mut Testbed) {
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
     let rad = 0.5;
 
     let positions = [vector![5.0, -1.0], vector![-5.0, -1.0]];
 
-    let platform_handles = std::array::IntoIter::new(positions)
+    let platform_handles = positions
+        .into_iter()
         .map(|pos| {
             let rigid_body = RigidBodyBuilder::new_kinematic_position_based()
                 .translation(pos)
@@ -57,7 +60,8 @@ pub fn init_world(testbed: &mut Testbed) {
                 handle,
                 &mut physics.islands,
                 &mut physics.colliders,
-                &mut physics.joints,
+                &mut physics.impulse_joints,
+                &mut physics.multibody_joints,
             );
 
             if let Some(graphics) = &mut graphics {
@@ -69,6 +73,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 0.0], 20.0);
 }
diff --git a/examples2d/ccd2.rs b/examples2d/ccd2.rs
index 1f25586..808c2e5 100644
--- a/examples2d/ccd2.rs
+++ b/examples2d/ccd2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -123,6 +124,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 2.5], 20.0);
 }
diff --git a/examples2d/collision_groups2.rs b/examples2d/collision_groups2.rs
index 3f2a786..110c32a 100644
--- a/examples2d/collision_groups2.rs
+++ b/examples2d/collision_groups2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -89,6 +90,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 1.0], 100.0);
 }
diff --git a/examples2d/convex_polygons2.rs b/examples2d/convex_polygons2.rs
index 4373fcb..060e0c5 100644
--- a/examples2d/convex_polygons2.rs
+++ b/examples2d/convex_polygons2.rs
@@ -9,7 +9,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -76,6 +77,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 50.0], 10.0);
 }
diff --git a/examples2d/damping2.rs b/examples2d/damping2.rs
index e308de6..481fee2 100644
--- a/examples2d/damping2.rs
+++ b/examples2d/damping2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Create the balls
@@ -38,6 +39,13 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world_with_params(bodies, colliders, joints, Vector::zeros(), ());
+    testbed.set_world_with_params(
+        bodies,
+        colliders,
+        impulse_joints,
+        multibody_joints,
+        Vector::zeros(),
+        (),
+    );
     testbed.look_at(point![3.0, 2.0], 50.0);
 }
diff --git a/examples2d/debug_box_ball2.rs b/examples2d/debug_box_ball2.rs
index 63b6f95..8f15b7a 100644
--- a/examples2d/debug_box_ball2.rs
+++ b/examples2d/debug_box_ball2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -33,6 +34,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 0.0], 50.0);
 }
diff --git a/examples2d/heightfield2.rs b/examples2d/heightfield2.rs
index a965ad8..09c604d 100644
--- a/examples2d/heightfield2.rs
+++ b/examples2d/heightfield2.rs
@@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -63,6 +64,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 0.0], 10.0);
 }
diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs
index 9333184..91d86cd 100644
--- a/examples2d/joints2.rs
+++ b/examples2d/joints2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Create the balls
@@ -44,16 +45,16 @@ pub fn init_world(testbed: &mut Testbed) {
             // Vertical joint.
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
-                let joint = BallJoint::new(Point::origin(), point![0.0, shift]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = RevoluteJoint::new().local_anchor2(point![0.0, shift]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             // Horizontal joint.
             if k > 0 {
                 let parent_index = body_handles.len() - numi;
                 let parent_handle = body_handles[parent_index];
-                let joint = BallJoint::new(Point::origin(), point![-shift, 0.0]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = RevoluteJoint::new().local_anchor2(point![-shift, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             body_handles.push(child_handle);
@@ -63,6 +64,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![numk as f32 * rad, numi as f32 * -rad], 20.0);
 }
diff --git a/examples2d/locked_rotations2.rs b/examples2d/locked_rotations2.rs
index a1f7ba5..32c528f 100644
--- a/examples2d/locked_rotations2.rs
+++ b/examples2d/locked_rotations2.rs
@@ -10,7 +10,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * The ground
@@ -51,6 +52,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 0.0], 40.0);
 }
diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs
index 6cbb2f0..2bed57d 100644
--- a/examples2d/one_way_platforms2.rs
+++ b/examples2d/one_way_platforms2.rs
@@ -62,7 +62,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -126,7 +127,8 @@ pub fn init_world(testbed: &mut Testbed) {
     testbed.set_world_with_params(
         bodies,
         colliders,
-        joints,
+        impulse_joints,
+        multibody_joints,
         vector![0.0, -9.81],
         physics_hooks,
     );
diff --git a/examples2d/platform2.rs b/examples2d/platform2.rs
index 48129a0..a737e34 100644
--- a/examples2d/platform2.rs
+++ b/examples2d/platform2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground.
@@ -89,6 +90,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Run the simulation.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 1.0], 40.0);
 }
diff --git a/examples2d/polyline2.rs b/examples2d/polyline2.rs
index 9f5241c..2cde60c 100644
--- a/examples2d/polyline2.rs
+++ b/examples2d/polyline2.rs
@@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -65,6 +66,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 0.0], 10.0);
 }
diff --git a/examples2d/pyramid2.rs b/examples2d/pyramid2.rs
index 76616be..04a7953 100644
--- a/examples2d/pyramid2.rs
+++ b/examples2d/pyramid2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -50,6 +51,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 2.5], 20.0);
 }
diff --git a/examples2d/restitution2.rs b/examples2d/restitution2.rs
index 1be4298..2650ec2 100644
--- a/examples2d/restitution2.rs
+++ b/examples2d/restitution2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -44,6 +45,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 1.0], 25.0);
 }
diff --git a/examples2d/sensor2.rs b/examples2d/sensor2.rs
index 5dc5940..9ef5a6b 100644
--- a/examples2d/sensor2.rs
+++ b/examples2d/sensor2.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground.
@@ -98,6 +99,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 1.0], 100.0);
 }
diff --git a/examples2d/trimesh2.rs b/examples2d/trimesh2.rs
index a4b049a..1474b77 100644
--- a/examples2d/trimesh2.rs
+++ b/examples2d/trimesh2.rs
@@ -13,7 +13,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -103,7 +104,7 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 20.0], 17.0);
 }
 
diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml
index 897c92b..bbab27e 100644
--- a/examples3d/Cargo.toml
+++ b/examples3d/Cargo.toml
@@ -2,7 +2,7 @@
 name    = "rapier-examples-3d"
 version = "0.1.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
-edition = "2018"
+edition = "2021"
 default-run = "all_examples3"
 
 [features]
@@ -20,10 +20,10 @@ wasm-bindgen = "0.2"
 obj-rs = { version =  "0.6", default-features = false }
 
 [dependencies.rapier_testbed3d]
-path = "../build/rapier_testbed3d"
+path = "../crates/rapier_testbed3d"
 
 [dependencies.rapier3d]
-path = "../build/rapier3d"
+path = "../crates/rapier3d"
 
 [[bin]]
 name = "all_examples3"
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index a9e1456..0aad4c2 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -8,6 +8,7 @@ use inflector::Inflector;
 use rapier_testbed3d::{Testbed, TestbedApp};
 use std::cmp::Ordering;
 
+mod articulations3;
 mod ccd3;
 mod collision_groups3;
 mod compound3;
@@ -15,6 +16,7 @@ mod convex_decomposition3;
 mod convex_polyhedron3;
 mod damping3;
 mod debug_add_remove_collider3;
+mod debug_articulations3;
 mod debug_big_colliders3;
 mod debug_boxes3;
 mod debug_cylinder3;
@@ -29,7 +31,7 @@ mod debug_trimesh3;
 mod domino3;
 mod fountain3;
 mod heightfield3;
-mod joints3;
+// mod joints3;
 mod keva3;
 mod locked_rotations3;
 mod one_way_platforms3;
@@ -78,6 +80,10 @@ pub fn main() {
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
         ("Fountain", fountain3::init_world),
         ("Primitives", primitives3::init_world),
+        (
+            "Multibody joints",
+            articulations3::init_world_with_articulations,
+        ),
         ("CCD", ccd3::init_world),
         ("Collision groups", collision_groups3::init_world),
         ("Compound", compound3::init_world),
@@ -86,7 +92,7 @@ pub fn main() {
         ("Damping", damping3::init_world),
         ("Domino", domino3::init_world),
         ("Heightfield", heightfield3::init_world),
-        ("Joints", joints3::init_world),
+        ("Impulse Joints", articulations3::init_world_with_joints),
         ("Locked rotations", locked_rotations3::init_world),
         ("One-way platforms", one_way_platforms3::init_world),
         ("Platform", platform3::init_world),
@@ -94,6 +100,7 @@ pub fn main() {
         ("Sensor", sensor3::init_world),
         ("TriMesh", trimesh3::init_world),
         ("Keva tower", keva3::init_world),
+        ("(Debug) multibody_joints", debug_articulations3::init_world),
         (
             "(Debug) add/rm collider",
             debug_add_remove_collider3::init_world,
diff --git a/examples3d/articulations3.rs b/examples3d/articulations3.rs
new file mode 100644
index 0000000..0c87c41
--- /dev/null
+++ b/examples3d/articulations3.rs
@@ -0,0 +1,678 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+fn create_prismatic_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    num: usize,
+    use_articulations: bool,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    let ground = RigidBodyBuilder::new_static()
+        .translation(vector![origin.x, origin.y, origin.z])
+        .build();
+    let mut curr_parent = bodies.insert(ground);
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    colliders.insert_with_parent(collider, curr_parent, bodies);
+
+    for i in 0..num {
+        let z = origin.z + (i + 1) as f32 * shift;
+        let rigid_body = RigidBodyBuilder::new_dynamic()
+            .translation(vector![origin.x, origin.y, z])
+            .build();
+        let curr_child = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+        colliders.insert_with_parent(collider, curr_child, bodies);
+
+        let axis = if i % 2 == 0 {
+            UnitVector::new_normalize(vector![1.0f32, 1.0, 0.0])
+        } else {
+            UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
+        };
+
+        let prism = PrismaticJoint::new(axis)
+            .local_anchor1(point![0.0, 0.0, 0.0])
+            .local_anchor2(point![0.0, 0.0, -shift])
+            .limit_axis([-2.0, 2.0]);
+
+        if use_articulations {
+            multibody_joints.insert(curr_parent, curr_child, prism);
+        } else {
+            impulse_joints.insert(curr_parent, curr_child, prism);
+        }
+        curr_parent = curr_child;
+    }
+}
+
+fn create_actuated_prismatic_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    num: usize,
+    use_articulations: bool,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    let ground = RigidBodyBuilder::new_static()
+        .translation(vector![origin.x, origin.y, origin.z])
+        .build();
+    let mut curr_parent = bodies.insert(ground);
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    colliders.insert_with_parent(collider, curr_parent, bodies);
+
+    for i in 0..num {
+        let z = origin.z + (i + 1) as f32 * shift;
+        let rigid_body = RigidBodyBuilder::new_dynamic()
+            .translation(vector![origin.x, origin.y, z])
+            .build();
+        let curr_child = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+        colliders.insert_with_parent(collider, curr_child, bodies);
+
+        let axis = if i % 2 == 0 {
+            UnitVector::new_normalize(vector![1.0, 1.0, 0.0])
+        } else {
+            UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
+        };
+
+        let mut prism = PrismaticJoint::new(axis)
+            .local_anchor1(point![0.0, 0.0, 0.0])
+            .local_anchor2(point![0.0, 0.0, -shift]);
+
+        if i == 1 {
+            prism = prism
+                .limit_axis([-Real::MAX, 5.0])
+                .motor_velocity(1.0, 1.0)
+                // We set a max impulse so that the motor doesn't fight
+                // the limits with large forces.
+                .motor_max_impulse(1.0);
+        } else if i > 1 {
+            prism = prism.motor_position(2.0, 0.01, 1.0);
+        } else {
+            prism = prism
+                .motor_velocity(1.0, 1.0)
+                // We set a max impulse so that the motor doesn't fight
+                // the limits with large forces.
+                .motor_max_impulse(0.7)
+                .limit_axis([-2.0, 5.0]);
+        }
+
+        if use_articulations {
+            multibody_joints.insert(curr_parent, curr_child, prism);
+        } else {
+            impulse_joints.insert(curr_parent, curr_child, prism);
+        }
+
+        curr_parent = curr_child;
+    }
+}
+
+fn create_revolute_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    num: usize,
+    use_articulations: bool,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    let ground = RigidBodyBuilder::new_static()
+        .translation(vector![origin.x, origin.y, 0.0])
+        .build();
+    let mut curr_parent = bodies.insert(ground);
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    colliders.insert_with_parent(collider, curr_parent, bodies);
+
+    for i in 0..num {
+        // Create four bodies.
+        let z = origin.z + i as f32 * shift * 2.0 + shift;
+        let positions = [
+            Isometry::translation(origin.x, origin.y, z),
+            Isometry::translation(origin.x + shift, origin.y, z),
+            Isometry::translation(origin.x + shift, origin.y, z + shift),
+            Isometry::translation(origin.x, origin.y, z + shift),
+        ];
+
+        let mut handles = [curr_parent; 4];
+        for k in 0..4 {
+            let rigid_body = RigidBodyBuilder::new_dynamic()
+                .position(positions[k])
+                .build();
+            handles[k] = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+            colliders.insert_with_parent(collider, handles[k], bodies);
+        }
+
+        // Setup four impulse_joints.
+        let x = Vector::x_axis();
+        let z = Vector::z_axis();
+        let revs = [
+            RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]),
+            RevoluteJoint::new(x).local_anchor2(point![-shift, 0.0, 0.0]),
+            RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]),
+            RevoluteJoint::new(x).local_anchor2(point![shift, 0.0, 0.0]),
+        ];
+
+        if use_articulations {
+            multibody_joints.insert(curr_parent, handles[0], revs[0]);
+            multibody_joints.insert(handles[0], handles[1], revs[1]);
+            multibody_joints.insert(handles[1], handles[2], revs[2]);
+            multibody_joints.insert(handles[2], handles[3], revs[3]);
+        } else {
+            impulse_joints.insert(curr_parent, handles[0], revs[0]);
+            impulse_joints.insert(handles[0], handles[1], revs[1]);
+            impulse_joints.insert(handles[1], handles[2], revs[2]);
+            impulse_joints.insert(handles[2], handles[3], revs[3]);
+        }
+
+        curr_parent = handles[3];
+    }
+}
+
+fn create_revolute_joints_with_limits(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    use_articulations: bool,
+) {
+    let ground = bodies.insert(
+        RigidBodyBuilder::new_static()
+            .translation(origin.coords)
+            .build(),
+    );
+
+    let platform1 = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(origin.coords)
+            .build(),
+    );
+    colliders.insert_with_parent(
+        ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
+        platform1,
+        bodies,
+    );
+
+    let shift = vector![0.0, 0.0, 6.0];
+    let platform2 = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(origin.coords + shift)
+            .build(),
+    );
+    colliders.insert_with_parent(
+        ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
+        platform2,
+        bodies,
+    );
+
+    let z = Vector::z_axis();
+    let joint1 = RevoluteJoint::new(z).limit_axis([-0.2, 0.2]);
+
+    if use_articulations {
+        multibody_joints.insert(ground, platform1, joint1);
+    } else {
+        impulse_joints.insert(ground, platform1, joint1);
+    }
+
+    let joint2 = RevoluteJoint::new(z)
+        .local_anchor2(-Point::from(shift))
+        .limit_axis([-0.2, 0.2]);
+
+    if use_articulations {
+        multibody_joints.insert(platform1, platform2, joint2);
+    } else {
+        impulse_joints.insert(platform1, platform2, joint2);
+    }
+
+    // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits.
+    let cuboid_body1 = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(origin.coords + vector![-2.0, 4.0, 0.0])
+            .build(),
+    );
+    colliders.insert_with_parent(
+        ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(),
+        cuboid_body1,
+        bodies,
+    );
+
+    let cuboid_body2 = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(origin.coords + shift + vector![2.0, 16.0, 0.0])
+            .build(),
+    );
+    colliders.insert_with_parent(
+        ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(),
+        cuboid_body2,
+        bodies,
+    );
+}
+
+fn create_fixed_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    num: usize,
+    use_articulations: bool,
+) {
+    let rad = 0.4;
+    let shift = 1.0;
+
+    let mut body_handles = Vec::new();
+
+    for i in 0..num {
+        for k in 0..num {
+            let fk = k as f32;
+            let fi = i as f32;
+
+            // NOTE: the num - 2 test is to avoid two consecutive
+            // fixed bodies. Because physx will crash if we add
+            // a joint between these.
+            let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) {
+                RigidBodyType::Static
+            } else {
+                RigidBodyType::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(status)
+                .translation(vector![
+                    origin.x + fk * shift,
+                    origin.y,
+                    origin.z + fi * shift
+                ])
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad).build();
+            colliders.insert_with_parent(collider, child_handle, bodies);
+
+            // Vertical joint.
+            if i > 0 {
+                let parent_index = body_handles.len() - num;
+                let parent_handle = body_handles[parent_index];
+                let joint = FixedJoint::new().local_anchor2(point![0.0, 0.0, -shift]);
+
+                if use_articulations {
+                    multibody_joints.insert(parent_handle, child_handle, joint);
+                } else {
+                    impulse_joints.insert(parent_handle, child_handle, joint);
+                }
+            }
+
+            // Horizontal joint.
+            if k > 0 {
+                let parent_index = body_handles.len() - 1;
+                let parent_handle = body_handles[parent_index];
+                let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+}
+
+fn create_spherical_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    num: usize,
+    use_articulations: bool,
+) {
+    let rad = 0.4;
+    let shift = 1.0;
+
+    let mut body_handles = Vec::new();
+
+    for k in 0..num {
+        for i in 0..num {
+            let fk = k as f32;
+            let fi = i as f32;
+
+            let status = if i == 0 && (k % 4 == 0 || k == num - 1) {
+                RigidBodyType::Static
+            } else {
+                RigidBodyType::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(status)
+                .translation(vector![fk * shift, 0.0, fi * shift * 2.0])
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build();
+            colliders.insert_with_parent(collider, child_handle, bodies);
+
+            // Vertical joint.
+            if i > 0 {
+                let parent_handle = *body_handles.last().unwrap();
+                let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
+
+                if use_articulations {
+                    multibody_joints.insert(parent_handle, child_handle, joint);
+                } else {
+                    impulse_joints.insert(parent_handle, child_handle, joint);
+                }
+            }
+
+            // Horizontal joint.
+            if k > 0 {
+                let parent_index = body_handles.len() - num;
+                let parent_handle = body_handles[parent_index];
+                let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+}
+
+fn create_spherical_joints_with_limits(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    use_articulations: bool,
+) {
+    let shift = vector![0.0, 0.0, 3.0];
+
+    let ground = bodies.insert(
+        RigidBodyBuilder::new_static()
+            .translation(origin.coords)
+            .build(),
+    );
+
+    let ball1 = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(origin.coords + shift)
+            .linvel(vector![20.0, 20.0, 0.0])
+            .build(),
+    );
+    colliders.insert_with_parent(
+        ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
+        ball1,
+        bodies,
+    );
+
+    let ball2 = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(origin.coords + shift * 2.0)
+            .build(),
+    );
+    colliders.insert_with_parent(
+        ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
+        ball2,
+        bodies,
+    );
+
+    let joint1 = SphericalJoint::new()
+        .local_anchor2(Point::from(-shift))
+        .limit_axis(JointAxis::X, [-0.2, 0.2])
+        .limit_axis(JointAxis::Y, [-0.2, 0.2]);
+
+    let joint2 = SphericalJoint::new()
+        .local_anchor2(Point::from(-shift))
+        .limit_axis(JointAxis::X, [-0.3, 0.3])
+        .limit_axis(JointAxis::Y, [-0.3, 0.3]);
+
+    if use_articulations {
+        multibody_joints.insert(ground, ball1, joint1);
+        multibody_joints.insert(ball1, ball2, joint2);
+    } else {
+        impulse_joints.insert(ground, ball1, joint1);
+        impulse_joints.insert(ball1, ball2, joint2);
+    }
+}
+
+fn create_actuated_revolute_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    num: usize,
+    use_articulations: bool,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    // We will reuse this base configuration for all the impulse_joints here.
+    let z = Vector::z_axis();
+    let joint_template = RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]);
+
+    let mut parent_handle = RigidBodyHandle::invalid();
+
+    for i in 0..num {
+        let fi = i as f32;
+
+        // NOTE: the num - 2 test is to avoid two consecutive
+        // fixed bodies. Because physx will crash if we add
+        // a joint between these.
+        let status = if i == 0 {
+            RigidBodyType::Static
+        } else {
+            RigidBodyType::Dynamic
+        };
+
+        let shifty = (i >= 1) as u32 as f32 * -2.0;
+
+        let rigid_body = RigidBodyBuilder::new(status)
+            .translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift])
+            // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
+            .build();
+
+        let child_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build();
+        colliders.insert_with_parent(collider, child_handle, bodies);
+
+        if i > 0 {
+            let mut joint = joint_template.clone();
+
+            if i % 3 == 1 {
+                joint = joint.motor_velocity(-20.0, 0.1);
+            } else if i == num - 1 {
+                let stiffness = 0.2;
+                let damping = 1.0;
+                joint = joint.motor_position(3.14 / 2.0, stiffness, damping);
+            }
+
+            if i == 1 {
+                joint = joint
+                    .local_anchor2(point![0.0, 2.0, -shift])
+                    .motor_velocity(-2.0, 0.1);
+            }
+
+            if use_articulations {
+                multibody_joints.insert(parent_handle, child_handle, joint);
+            } else {
+                impulse_joints.insert(parent_handle, child_handle, joint);
+            }
+        }
+
+        parent_handle = child_handle;
+    }
+}
+
+fn create_actuated_spherical_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    num: usize,
+    use_articulations: bool,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    // We will reuse this base configuration for all the impulse_joints here.
+    let joint_template = SphericalJoint::new().local_anchor1(point![0.0, 0.0, shift]);
+
+    let mut parent_handle = RigidBodyHandle::invalid();
+
+    for i in 0..num {
+        let fi = i as f32;
+
+        // NOTE: the num - 2 test is to avoid two consecutive
+        // fixed bodies. Because physx will crash if we add
+        // a joint between these.
+        let status = if i == 0 {
+            RigidBodyType::Static
+        } else {
+            RigidBodyType::Dynamic
+        };
+
+        let rigid_body = RigidBodyBuilder::new(status)
+            .translation(vector![origin.x, origin.y, origin.z + fi * shift])
+            // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
+            .build();
+
+        let child_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build();
+        colliders.insert_with_parent(collider, child_handle, bodies);
+
+        if i > 0 {
+            let mut joint = joint_template.clone();
+
+            if i == 1 {
+                joint = joint
+                    .motor_velocity(JointAxis::AngX, 0.0, 0.1)
+                    .motor_velocity(JointAxis::AngY, 0.5, 0.1)
+                    .motor_velocity(JointAxis::AngZ, -2.0, 0.1);
+            } else if i == num - 1 {
+                let stiffness = 0.2;
+                let damping = 1.0;
+                joint = joint
+                    .motor_position(JointAxis::AngX, 0.0, stiffness, damping)
+                    .motor_position(JointAxis::AngY, 1.0, stiffness, damping)
+                    .motor_position(JointAxis::AngZ, 3.14 / 2.0, stiffness, damping);
+            }
+
+            if use_articulations {
+                multibody_joints.insert(parent_handle, child_handle, joint);
+            } else {
+                impulse_joints.insert(parent_handle, child_handle, joint);
+            }
+        }
+
+        parent_handle = child_handle;
+    }
+}
+
+fn do_init_world(testbed: &mut Testbed, use_articulations: bool) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let mut multibody_joints = MultibodyJointSet::new();
+
+    create_prismatic_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![20.0, 5.0, 0.0],
+        4,
+        use_articulations,
+    );
+    create_actuated_prismatic_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![25.0, 5.0, 0.0],
+        4,
+        use_articulations,
+    );
+    create_revolute_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![20.0, 0.0, 0.0],
+        3,
+        use_articulations,
+    );
+    create_revolute_joints_with_limits(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![34.0, 0.0, 0.0],
+        use_articulations,
+    );
+    create_fixed_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![0.0, 10.0, 0.0],
+        10,
+        use_articulations,
+    );
+    create_actuated_revolute_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![20.0, 10.0, 0.0],
+        6,
+        use_articulations,
+    );
+    create_actuated_spherical_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![13.0, 10.0, 0.0],
+        3,
+        use_articulations,
+    );
+    create_spherical_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        15,
+        use_articulations,
+    );
+    create_spherical_joints_with_limits(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![-5.0, 0.0, 0.0],
+        use_articulations,
+    );
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
+}
+
+pub fn init_world_with_joints(testbed: &mut Testbed) {
+    do_init_world(testbed, false)
+}
+
+pub fn init_world_with_articulations(testbed: &mut Testbed) {
+    do_init_world(testbed, true)
+}
diff --git a/examples3d/ccd3.rs b/examples3d/ccd3.rs
index 8d676e3..2e00b86 100644
--- a/examples3d/ccd3.rs
+++ b/examples3d/ccd3.rs
@@ -44,7 +44,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -153,6 +154,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/examples3d/collision_groups3.rs b/examples3d/collision_groups3.rs
index 8db86fa..d43a7fc 100644
--- a/examples3d/collision_groups3.rs
+++ b/examples3d/collision_groups3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -93,6 +94,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point!(10.0, 10.0, 10.0), Point::origin());
 }
diff --git a/examples3d/compound3.rs b/examples3d/compound3.rs
index f5f1de1..433210d 100644
--- a/examples3d/compound3.rs
+++ b/examples3d/compound3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -90,6 +91,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/examples3d/convex_decomposition3.rs b/examples3d/convex_decomposition3.rs
index 64c8802..98cf4a9 100644
--- a/examples3d/convex_decomposition3.rs
+++ b/examples3d/convex_decomposition3.rs
@@ -15,7 +15,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -52,7 +53,6 @@ pub fn init_world(testbed: &mut Testbed) {
                 .iter()
                 .map(|v| point![v.0, v.1, v.2])
                 .collect();
-            use std::iter::FromIterator;
             let indices: Vec<_> = model
                 .polygons
                 .into_iter()
@@ -104,7 +104,7 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
 
diff --git a/examples3d/convex_polyhedron3.rs b/examples3d/convex_polyhedron3.rs
index 896eb03..7abcefb 100644
--- a/examples3d/convex_polyhedron3.rs
+++ b/examples3d/convex_polyhedron3.rs
@@ -9,7 +9,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -68,6 +69,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![30.0, 30.0, 30.0], Point::origin());
 }
diff --git a/examples3d/damping3.rs b/examples3d/damping3.rs
index c634a0a..6f2edb9 100644
--- a/examples3d/damping3.rs
+++ b/examples3d/damping3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Create the cubes
@@ -38,6 +39,13 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world_with_params(bodies, colliders, joints, Vector::zeros(), ());
+    testbed.set_world_with_params(
+        bodies,
+        colliders,
+        impulse_joints,
+        multibody_joints,
+        Vector::zeros(),
+        (),
+    );
     testbed.look_at(point![2.0, 2.5, 20.0], point![2.0, 2.5, 0.0]);
 }
diff --git a/examples3d/debug_add_remove_collider3.rs b/examples3d/debug_add_remove_collider3.rs
index 12d58ec..dd680d5 100644
--- a/examples3d/debug_add_remove_collider3.rs
+++ b/examples3d/debug_add_remove_collider3.rs
@@ -7,7 +7,9 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
     /*
      * Ground.
      */
@@ -54,6 +56,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
 }
diff --git a/examples3d/debug_articulations3.rs b/examples3d/debug_articulations3.rs
new file mode 100644
index 0000000..26831ef
--- /dev/null
+++ b/examples3d/debug_articulations3.rs
@@ -0,0 +1,98 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+fn create_ball_articulations(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    num: usize,
+) {
+    let rad = 0.4;
+    let shift = 1.0;
+
+    let mut body_handles = Vec::new();
+
+    for k in 0..num {
+        for i in 0..num {
+            let fk = k as f32;
+            let fi = i as f32;
+
+            let status = if i == 0 {
+                // && (k % 4 == 0 || k == num - 1) {
+                RigidBodyType::Static
+            } else {
+                RigidBodyType::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(status)
+                .translation(vector![fk * shift, 0.0, fi * shift * 2.0])
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build();
+            colliders.insert_with_parent(collider, child_handle, bodies);
+
+            // Vertical multibody_joint.
+            if i > 0 {
+                let parent_handle = *body_handles.last().unwrap();
+                let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
+                multibody_joints.insert(parent_handle, child_handle, joint);
+            }
+
+            // Horizontal multibody_joint.
+            if k > 0 && i > 0 {
+                let parent_index = body_handles.len() - num;
+                let parent_handle = body_handles[parent_index];
+                let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
+                // let joint =
+                //     PrismaticJoint::new(Vector::y_axis()).local_anchor2(point![-shift, 0.0, 0.0]);
+                // let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
+                // let joint =
+                //     RevoluteJoint::new(Vector::x_axis()).local_anchor2(point![-shift, 0.0, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let mut multibody_joints = MultibodyJointSet::new();
+
+    let rigid_body = RigidBodyBuilder::new_dynamic().build();
+    let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) // Vector::y_axis())
+        .translation(vector![0.0, -3.0, 0.0])
+        .rotation(vector![0.1, 0.0, 0.1])
+        .build();
+    let handle = bodies.insert(rigid_body);
+    colliders.insert_with_parent(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) // Vector::y_axis())
+        .translation(vector![0.0, -3.02, 0.0])
+        .rotation(vector![0.1, 0.0, 0.1])
+        .build();
+    let handle = bodies.insert(rigid_body);
+    colliders.insert_with_parent(collider, handle, &mut bodies);
+
+    create_ball_articulations(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        15,
+    );
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
+}
diff --git a/examples3d/debug_big_colliders3.rs b/examples3d/debug_big_colliders3.rs
index 864782f..33460dc 100644
--- a/examples3d/debug_big_colliders3.rs
+++ b/examples3d/debug_big_colliders3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -38,6 +39,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
 }
diff --git a/examples3d/debug_boxes3.rs b/examples3d/debug_boxes3.rs
index ea39a8a..ddf86db 100644
--- a/examples3d/debug_boxes3.rs
+++ b/examples3d/debug_boxes3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -25,10 +26,10 @@ pub fn init_world(testbed: &mut Testbed) {
     }
 
     // Build the dynamic box rigid body.
-    for _ in 0..6 {
+    for _ in 0..2 {
         let rigid_body = RigidBodyBuilder::new_dynamic()
             .translation(vector![1.1, 0.0, 0.0])
-            .rotation(vector![0.8, 0.2, 0.1])
+            // .rotation(vector![0.8, 0.2, 0.1])
             .can_sleep(false)
             .build();
         let handle = bodies.insert(rigid_body);
@@ -39,6 +40,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
 }
diff --git a/examples3d/debug_cylinder3.rs b/examples3d/debug_cylinder3.rs
index 88908c1..cb087be 100644
--- a/examples3d/debug_cylinder3.rs
+++ b/examples3d/debug_cylinder3.rs
@@ -10,7 +10,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -55,6 +56,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/examples3d/debug_dynamic_collider_add3.rs b/examples3d/debug_dynamic_collider_add3.rs
index f66d8ce..27e1b0b 100644
--- a/examples3d/debug_dynamic_collider_add3.rs
+++ b/examples3d/debug_dynamic_collider_add3.rs
@@ -7,7 +7,9 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
     /*
      * Ground.
      */
@@ -114,6 +116,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
 }
diff --git a/examples3d/debug_friction3.rs b/examples3d/debug_friction3.rs
index 7d01986..44c9fae 100644
--- a/examples3d/debug_friction3.rs
+++ b/examples3d/debug_friction3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -38,6 +39,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/examples3d/debug_infinite_fall3.rs b/examples3d/debug_infinite_fall3.rs
index dcf4f12..4d950cf 100644
--- a/examples3d/debug_infinite_fall3.rs
+++ b/examples3d/debug_infinite_fall3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -44,5 +45,5 @@ pub fn init_world(testbed: &mut Testbed) {
      * Set up the testbed.
      */
     testbed.look_at(point![100.0, -10.0, 100.0], Point::origin());
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
 }
diff --git a/examples3d/debug_prismatic3.rs b/examples3d/debug_prismatic3.rs
index 43c6cc0..15176f3 100644
--- a/examples3d/debug_prismatic3.rs
+++ b/examples3d/debug_prismatic3.rs
@@ -4,7 +4,7 @@ use rapier_testbed3d::Testbed;
 fn prismatic_repro(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     box_center: Point<f32>,
 ) {
     let box_rb = bodies.insert(
@@ -39,19 +39,12 @@ fn prismatic_repro(
         );
         colliders.insert_with_parent(ColliderBuilder::ball(0.5).build(), wheel_rb, bodies);
 
-        let mut prismatic = rapier3d::dynamics::PrismaticJoint::new(
-            point![pos.x, pos.y, pos.z],
-            Vector::y_axis(),
-            Vector::zeros(),
-            Point::origin(),
-            Vector::y_axis(),
-            Vector::default(),
-        );
-        prismatic.configure_motor_model(rapier3d::dynamics::SpringModel::VelocityBased);
         let (stiffness, damping) = (0.05, 0.2);
-        prismatic.configure_motor_position(0.0, stiffness, damping);
 
-        joints.insert(box_rb, wheel_rb, prismatic);
+        let prismatic = PrismaticJoint::new(Vector::y_axis())
+            .local_anchor1(point![pos.x, pos.y, pos.z])
+            .motor_position(0.0, stiffness, damping);
+        impulse_joints.insert(box_rb, wheel_rb, prismatic);
     }
 
     // put a small box under one of the wheels
@@ -73,7 +66,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -91,13 +85,13 @@ pub fn init_world(testbed: &mut Testbed) {
     prismatic_repro(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![0.0, 5.0, 0.0],
     );
 
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
 }
diff --git a/examples3d/debug_rollback3.rs b/examples3d/debug_rollback3.rs
index c5e5bde..b795434 100644
--- a/examples3d/debug_rollback3.rs
+++ b/examples3d/debug_rollback3.rs
@@ -7,7 +7,9 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
     /*
      * Ground.
      */
@@ -65,6 +67,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
 }
diff --git a/examples3d/debug_shape_modification3.rs b/examples3d/debug_shape_modification3.rs
index 6c27288..eb2966e 100644
--- a/examples3d/debug_shape_modification3.rs
+++ b/examples3d/debug_shape_modification3.rs
@@ -7,7 +7,9 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
     /*
      * Ground.
      */
@@ -113,6 +115,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
 }
diff --git a/examples3d/debug_triangle3.rs b/examples3d/debug_triangle3.rs
index 0c40882..8151cf9 100644
--- a/examples3d/debug_triangle3.rs
+++ b/examples3d/debug_triangle3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     // Triangle ground.
     let vtx = [
@@ -36,6 +37,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
 }
diff --git a/examples3d/debug_trimesh3.rs b/examples3d/debug_trimesh3.rs
index d21d0d3..8e38719 100644
--- a/examples3d/debug_trimesh3.rs
+++ b/examples3d/debug_trimesh3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     // Triangle ground.
     let width = 0.5;
@@ -57,6 +58,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
 }
diff --git a/examples3d/domino3.rs b/examples3d/domino3.rs
index 7e9143f..067a86d 100644
--- a/examples3d/domino3.rs
+++ b/examples3d/domino3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/examples3d/fountain3.rs b/examples3d/fountain3.rs
index 788d05d..4e47878 100644
--- a/examples3d/fountain3.rs
+++ b/examples3d/fountain3.rs
@@ -6,7 +6,9 @@ const MAX_NUMBER_OF_BODIES: usize = 400;
 pub fn init_world(testbed: &mut Testbed) {
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
     let rad = 0.5;
 
     /*
@@ -63,7 +65,8 @@ pub fn init_world(testbed: &mut Testbed) {
                     *handle,
                     &mut physics.islands,
                     &mut physics.colliders,
-                    &mut physics.joints,
+                    &mut physics.impulse_joints,
+                    &mut physics.multibody_joints,
                 );
 
                 if let Some(graphics) = &mut graphics {
@@ -76,10 +79,10 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     // testbed
     //     .physics_state_mut()
     //     .integration_parameters
-    //     .velocity_based_erp = 0.2;
+    //     .erp = 0.2;
     testbed.look_at(point![-30.0, 4.0, -30.0], point![0.0, 1.0, 0.0]);
 }
diff --git a/examples3d/harness_capsules3.rs b/examples3d/harness_capsules3.rs
index e2f19d5..dae364a 100644
--- a/examples3d/harness_capsules3.rs
+++ b/examples3d/harness_capsules3.rs
@@ -7,7 +7,8 @@ pub fn init_world(harness: &mut Harness) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -59,7 +60,7 @@ pub fn init_world(harness: &mut Harness) {
     /*
      * Set up the harness.
      */
-    harness.set_world(bodies, colliders, joints);
+    harness.set_world(bodies, colliders, impulse_joints, multibody_joints);
 }
 
 fn main() {
diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs
index bb93667..f5a3ba6 100644
--- a/examples3d/heightfield3.rs
+++ b/examples3d/heightfield3.rs
@@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -95,6 +96,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs
index 11cd533..02aa5f9 100644
--- a/examples3d/joints3.rs
+++ b/examples3d/joints3.rs
@@ -4,7 +4,7 @@ use rapier_testbed3d::Testbed;
 fn create_prismatic_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
@@ -28,25 +28,17 @@ fn create_prismatic_joints(
         colliders.insert_with_parent(collider, curr_child, bodies);
 
         let axis = if i % 2 == 0 {
-            UnitVector::new_normalize(vector![1.0, 1.0, 0.0])
+            UnitVector::new_normalize(vector![1.0f32, 1.0, 0.0])
         } else {
-            UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
+            UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
         };
 
-        let z = Vector::z();
-        let mut prism = PrismaticJoint::new(
-            point![0.0, 0.0, 0.0],
-            axis,
-            z,
-            point![0.0, 0.0, -shift],
-            axis,
-            z,
-        );
-        prism.limits_enabled = true;
-        prism.limits[0] = -2.0;
-        prism.limits[1] = 2.0;
+        let mut prism = JointData::prismatic(axis)
+            .local_anchor1(point![0.0, 0.0, shift])
+            .local_anchor2(point![0.0, 0.0, 0.0])
+            .limit_axis(JointAxis::X, [-2.0, 2.0]);
 
-        joints.insert(curr_parent, curr_child, prism);
+        impulse_joints.insert(curr_parent, curr_child, prism);
 
         curr_parent = curr_child;
     }
@@ -55,7 +47,7 @@ fn create_prismatic_joints(
 fn create_actuated_prismatic_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
@@ -84,36 +76,29 @@ fn create_actuated_prismatic_joints(
             UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
         };
 
-        let z = Vector::z();
-        let mut prism = PrismaticJoint::new(
-            point![0.0, 0.0, 0.0],
-            axis,
-            z,
-            point![0.0, 0.0, -shift],
-            axis,
-            z,
-        );
+        let mut prism = JointData::prismatic(axis)
+            .local_anchor1(point![0.0, 0.0, 0.0])
+            .local_anchor2(point![0.0, 0.0, -shift]);
 
         if i == 1 {
-            prism.configure_motor_velocity(1.0, 1.0);
-            prism.limits_enabled = true;
-            prism.limits[1] = 5.0;
-            // We set a max impulse so that the motor doesn't fight
-            // the limits with large forces.
-            prism.motor_max_impulse = 1.0;
+            prism = prism
+                .limit_axis(JointAxis::X, [-Real::MAX, 5.0])
+                .motor_velocity(JointAxis::X, 1.0, 1.0)
+                // We set a max impulse so that the motor doesn't fight
+                // the limits with large forces.
+                .motor_max_impulse(JointAxis::X, 1.0);
         } else if i > 1 {
-            prism.configure_motor_position(2.0, 0.01, 1.0);
+            prism = prism.motor_position(JointAxis::X, 2.0, 0.01, 1.0);
         } else {
-            prism.configure_motor_velocity(1.0, 1.0);
-            // We set a max impulse so that the motor doesn't fight
-            // the limits with large forces.
-            prism.motor_max_impulse = 0.7;
-            prism.limits_enabled = true;
-            prism.limits[0] = -2.0;
-            prism.limits[1] = 5.0;
+            prism = prism
+                .motor_velocity(JointAxis::X, 1.0, 1.0)
+                // We set a max impulse so that the motor doesn't fight
+                // the limits with large forces.
+                .motor_max_impulse(JointAxis::X, 0.7)
+                .limit_axis(JointAxis::X, [-2.0, 5.0]);
         }
 
-        joints.insert(curr_parent, curr_child, prism);
+        impulse_joints.insert(curr_parent, curr_child, prism);
 
         curr_parent = curr_child;
     }
@@ -122,7 +107,7 @@ fn create_actuated_prismatic_joints(
 fn create_revolute_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
@@ -156,22 +141,20 @@ fn create_revolute_joints(
             colliders.insert_with_parent(collider, handles[k], bodies);
         }
 
-        // Setup four joints.
-        let o = Point::origin();
+        // Setup four impulse_joints.
         let x = Vector::x_axis();
         let z = Vector::z_axis();
-
         let revs = [
-            RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
-            RevoluteJoint::new(o, x, point![-shift, 0.0, 0.0], x),
-            RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
-            RevoluteJoint::new(o, x, point![shift, 0.0, 0.0], x),
+            RevoluteJoint::new(x).local_anchor2(point![0.0, 0.0, -shift]),
+            RevoluteJoint::new(z).local_anchor2(point![-shift, 0.0, 0.0]),
+            RevoluteJoint::new(x).local_anchor2(point![0.0, 0.0, -shift]),
+            RevoluteJoint::new(z).local_anchor2(point![shift, 0.0, 0.0]),
         ];
 
-        joints.insert(curr_parent, handles[0], revs[0]);
-        joints.insert(handles[0], handles[1], revs[1]);
-        joints.insert(handles[1], handles[2], revs[2]);
-        joints.insert(handles[2], handles[3], revs[3]);
+        impulse_joints.insert(curr_parent, handles[0], revs[0]);
+        impulse_joints.insert(handles[0], handles[1], revs[1]);
+        impulse_joints.insert(handles[1], handles[2], revs[2]);
+        impulse_joints.insert(handles[2], handles[3], revs[3]);
 
         curr_parent = handles[3];
     }
@@ -180,7 +163,7 @@ fn create_revolute_joints(
 fn create_revolute_joints_with_limits(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
 ) {
     let ground = bodies.insert(
@@ -212,25 +195,14 @@ fn create_revolute_joints_with_limits(
         bodies,
     );
 
-    let mut joint1 = RevoluteJoint::new(
-        Point::origin(),
-        Vector::z_axis(),
-        Point::origin(),
-        Vector::z_axis(),
-    );
-    joint1.limits_enabled = true;
-    joint1.limits = [-0.2, 0.2];
-    joints.insert(ground, platform1, joint1);
+    let z = Vector::z_axis();
+    let mut joint1 = RevoluteJoint::new(z).limit_axis(JointAxis::X, [-0.2, 0.2]);
+    impulse_joints.insert(ground, platform1, joint1);
 
-    let mut joint2 = RevoluteJoint::new(
-        Point::origin(),
-        Vector::z_axis(),
-        Point::from(-shift),
-        Vector::z_axis(),
-    );
-    joint2.limits_enabled = true;
-    joint2.limits = [-0.3, 0.3];
-    joints.insert(platform1, platform2, joint2);
+    let mut joint2 = RevoluteJoint::new(z)
+        .local_anchor2(shift.into())
+        .limit_axis(JointAxis::Z, [-0.2, 0.2]);
+    impulse_joints.insert(platform1, platform2, joint2);
 
     // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits.
     let cuboid_body1 = bodies.insert(
@@ -259,7 +231,7 @@ fn create_revolute_joints_with_limits(
 fn create_fixed_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
@@ -268,8 +240,8 @@ fn create_fixed_joints(
 
     let mut body_handles = Vec::new();
 
-    for k in 0..num {
-        for i in 0..num {
+    for i in 0..num {
+        for k in 0..num {
             let fk = k as f32;
             let fi = i as f32;
 
@@ -295,23 +267,18 @@ fn create_fixed_joints(
 
             // Vertical joint.
             if i > 0 {
-                let parent_handle = *body_handles.last().unwrap();
-                let joint = FixedJoint::new(
-                    Isometry::identity(),
-                    Isometry::translation(0.0, 0.0, -shift),
-                );
-                joints.insert(parent_handle, child_handle, joint);
+                let parent_index = body_handles.len() - num;
+                let parent_handle = body_handles[parent_index];
+                let joint = JointData::fixed().local_anchor2(point![0.0, 0.0, -shift]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             // Horizontal joint.
             if k > 0 {
-                let parent_index = body_handles.len() - num;
+                let parent_index = body_handles.len() - 1;
                 let parent_handle = body_handles[parent_index];
-                let joint = FixedJoint::new(
-                    Isometry::identity(),
-                    Isometry::translation(-shift, 0.0, 0.0),
-                );
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = JointData::fixed().local_anchor2(point![-shift, 0.0, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             body_handles.push(child_handle);
@@ -322,7 +289,7 @@ fn create_fixed_joints(
 fn create_ball_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     num: usize,
 ) {
     let rad = 0.4;
@@ -351,16 +318,16 @@ fn create_ball_joints(
             // Vertical joint.
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
-                let joint = BallJoint::new(Point::origin(), point![0.0, 0.0, -shift * 2.0]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = JointData::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             // Horizontal joint.
             if k > 0 {
                 let parent_index = body_handles.len() - num;
                 let parent_handle = body_handles[parent_index];
-                let joint = BallJoint::new(Point::origin(), point![-shift, 0.0, 0.0]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = JointData::ball().local_anchor2(point![-shift, 0.0, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             body_handles.push(child_handle);
@@ -371,7 +338,7 @@ fn create_ball_joints(
 fn create_ball_joints_with_limits(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
 ) {
     let shift = vector![0.0, 0.0, 3.0];
@@ -405,38 +372,32 @@ fn create_ball_joints_with_limits(
         bodies,
     );
 
-    let mut joint1 = BallJoint::new(Point::origin(), Point::from(-shift));
-    joint1.limits_enabled = true;
-    joint1.limits_local_axis1 = Vector::z_axis();
-    joint1.limits_local_axis2 = Vector::z_axis();
-    joint1.limits_angle = 0.2;
-    joints.insert(ground, ball1, joint1);
+    let mut joint1 = JointData::ball()
+        .local_anchor2(Point::from(-shift))
+        .limit_axis(JointAxis::X, [-0.2, 0.2])
+        .limit_axis(JointAxis::Y, [-0.2, 0.2]);
+    impulse_joints.insert(ground, ball1, joint1);
 
-    let mut joint2 = BallJoint::new(Point::origin(), Point::from(-shift));
-    joint2.limits_enabled = true;
-    joint2.limits_local_axis1 = Vector::z_axis();
-    joint2.limits_local_axis2 = Vector::z_axis();
-    joint2.limits_angle = 0.3;
-    joints.insert(ball1, ball2, joint2);
+    let mut joint2 = JointData::ball()
+        .local_anchor2(Point::from(-shift))
+        .limit_axis(JointAxis::X, [-0.3, 0.3])
+        .limit_axis(JointAxis::Y, [-0.3, 0.3]);
+    impulse_joints.insert(ball1, ball2, joint2);
 }
 
 fn create_actuated_revolute_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
     let rad = 0.4;
     let shift = 2.0;
 
-    // We will reuse this base configuration for all the joints here.
-    let joint_template = RevoluteJoint::new(
-        Point::origin(),
-        Vector::z_axis(),
-        point![0.0, 0.0, -shift],
-        Vector::z_axis(),
-    );
+    // We will reuse this base configuration for all the impulse_joints here.
+    let z = Vector::z_axis();
+    let joint_template = RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]);
 
     let mut parent_handle = RigidBodyHandle::invalid();
 
@@ -467,19 +428,19 @@ fn create_actuated_revolute_joints(
             let mut joint = joint_template.clone();
 
             if i % 3 == 1 {
-                joint.configure_motor_velocity(-20.0, 0.1);
+                joint = joint.motor_velocity(JointAxis::AngX, -20.0, 0.1);
             } else if i == num - 1 {
                 let stiffness = 0.2;
                 let damping = 1.0;
-                joint.configure_motor_position(3.14 / 2.0, stiffness, damping);
+                joint = joint.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping);
             }
 
             if i == 1 {
-                joint.local_anchor2.y = 2.0;
-                joint.configure_motor_velocity(-2.0, 0.1);
+                joint.local_frame2.translation.vector.y = 2.0;
+                joint = joint.motor_velocity(JointAxis::AngX, -2.0, 0.1);
             }
 
-            joints.insert(parent_handle, child_handle, joint);
+            impulse_joints.insert(parent_handle, child_handle, joint);
         }
 
         parent_handle = child_handle;
@@ -489,15 +450,15 @@ fn create_actuated_revolute_joints(
 fn create_actuated_ball_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
     let rad = 0.4;
     let shift = 2.0;
 
-    // We will reuse this base configuration for all the joints here.
-    let joint_template = BallJoint::new(point![0.0, 0.0, shift], Point::origin());
+    // We will reuse this base configuration for all the impulse_joints here.
+    let joint_template = JointData::ball().local_anchor1(point![0.0, 0.0, shift]);
 
     let mut parent_handle = RigidBodyHandle::invalid();
 
@@ -526,18 +487,20 @@ fn create_actuated_ball_joints(
             let mut joint = joint_template.clone();
 
             if i == 1 {
-                joint.configure_motor_velocity(vector![0.0, 0.5, -2.0], 0.1);
+                joint = joint
+                    .motor_velocity(JointAxis::AngX, 0.0, 0.1)
+                    .motor_velocity(JointAxis::AngY, 0.5, 0.1)
+                    .motor_velocity(JointAxis::AngZ, -2.0, 0.1);
             } else if i == num - 1 {
                 let stiffness = 0.2;
                 let damping = 1.0;
-                joint.configure_motor_position(
-                    Rotation::new(vector![0.0, 1.0, 3.14 / 2.0]),
-                    stiffness,
-                    damping,
-                );
+                joint = joint
+                    .motor_position(JointAxis::AngX, 0.0, stiffness, damping)
+                    .motor_position(JointAxis::AngY, 1.0, stiffness, damping)
+                    .motor_position(JointAxis::AngZ, 3.14 / 2.0, stiffness, damping);
             }
 
-            joints.insert(parent_handle, child_handle, joint);
+            impulse_joints.insert(parent_handle, child_handle, joint);
         }
 
         parent_handle = child_handle;
@@ -550,67 +513,68 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     create_prismatic_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![20.0, 5.0, 0.0],
         4,
     );
     create_actuated_prismatic_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![25.0, 5.0, 0.0],
         4,
     );
     create_revolute_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![20.0, 0.0, 0.0],
         3,
     );
     create_revolute_joints_with_limits(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![34.0, 0.0, 0.0],
     );
     create_fixed_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![0.0, 10.0, 0.0],
         10,
     );
     create_actuated_revolute_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![20.0, 10.0, 0.0],
         6,
     );
     create_actuated_ball_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![13.0, 10.0, 0.0],
         3,
     );
-    create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15);
+    create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
     create_ball_joints_with_limits(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![-5.0, 0.0, 0.0],
     );
 
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
 }
diff --git a/examples3d/keva3.rs b/examples3d/keva3.rs
index 38a0432..ad9e1ae 100644
--- a/examples3d/keva3.rs
+++ b/examples3d/keva3.rs
@@ -83,7 +83,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -132,6 +133,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/examples3d/locked_rotations3.rs b/examples3d/locked_rotations3.rs
index 95dfae1..5e925b0 100644
--- a/examples3d/locked_rotations3.rs
+++ b/examples3d/locked_rotations3.rs
@@ -10,7 +10,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * The ground
@@ -54,6 +55,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![10.0, 3.0, 0.0], point![0.0, 3.0, 0.0]);
 }
diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs
index 3802563..4857ae6 100644
--- a/examples3d/one_way_platforms3.rs
+++ b/examples3d/one_way_platforms3.rs
@@ -62,7 +62,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -126,7 +127,8 @@ pub fn init_world(testbed: &mut Testbed) {
     testbed.set_world_with_params(
         bodies,
         colliders,
-        joints,
+        impulse_joints,
+        multibody_joints,
         vector![0.0, -9.81, 0.0],
         physics_hooks,
     );
diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs
index 6b3c234..d1126ec 100644
--- a/examples3d/platform3.rs
+++ b/examples3d/platform3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground.
@@ -103,6 +104,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Run the simulation.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![-10.0, 5.0, -10.0], Point::origin());
 }
diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs
index db9143b..891557e 100644
--- a/examples3d/primitives3.rs
+++ b/examples3d/primitives3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -51,13 +52,13 @@ pub fn init_world(testbed: &mut Testbed) {
                 let handle = bodies.insert(rigid_body);
 
                 let collider = match j % 5 {
-                    0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
-                    1 => ColliderBuilder::ball(rad).build(),
-                    // Rounded cylinders are much more efficient that cylinder, even if the
-                    // rounding margin is small.
-                    2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
-                    3 => ColliderBuilder::cone(rad, rad).build(),
-                    _ => ColliderBuilder::capsule_y(rad, rad).build(),
+                    _ => ColliderBuilder::cuboid(rad, rad, rad).build(),
+                    // 1 => ColliderBuilder::ball(rad).build(),
+                    // // Rounded cylinders are much more efficient that cylinder, even if the
+                    // // rounding margin is small.
+                    // 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
+                    // 3 => ColliderBuilder::cone(rad, rad).build(),
+                    // _ => ColliderBuilder::capsule_y(rad, rad).build(),
                 };
 
                 colliders.insert_with_parent(collider, handle, &mut bodies);
@@ -70,6 +71,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/examples3d/rapier.data b/examples3d/rapier.data
new file mode 100644
index 0000000..33ddaa3
Binary files /dev/null and b/examples3d/rapier.data differ
diff --git a/examples3d/restitution3.rs b/examples3d/restitution3.rs
index 4c08742..00462d1 100644
--- a/examples3d/restitution3.rs
+++ b/examples3d/restitution3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -44,6 +45,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 3.0, 30.0], point![0.0, 3.0, 0.0]);
 }
diff --git a/examples3d/sensor3.rs b/examples3d/sensor3.rs
index 0e2b489..ebd47f3 100644
--- a/examples3d/sensor3.rs
+++ b/examples3d/sensor3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground.
@@ -102,6 +103,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![-6.0, 4.0, -6.0], point![0.0, 1.0, 0.0]);
 }
diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs
index f5810aa..31a532b 100644
--- a/examples3d/trimesh3.rs
+++ b/examples3d/trimesh3.rs
@@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -100,6 +101,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/publish-testbeds.sh b/publish-testbeds.sh
index 2b2e068..24ef410 100755
--- a/publish-testbeds.sh
+++ b/publish-testbeds.sh
@@ -6,20 +6,20 @@ echo "$tmp"
 
 cp -r src "$tmp"/.
 cp -r src_testbed "$tmp"/.
-cp -r build "$tmp"/.
+cp -r crates "$tmp"/.
 cp -r LICENSE README.md "$tmp"/.
 
 ### Publish the 2D version.
-sed 's#\.\./\.\./src#src#g' build/rapier_testbed2d/Cargo.toml > "$tmp"/Cargo.toml
-sed -i 's#\.\./rapier#./build/rapier#g' "$tmp"/Cargo.toml
+sed 's#\.\./\.\./src#src#g' crates/rapier_testbed2d/Cargo.toml > "$tmp"/Cargo.toml
+sed -i 's#\.\./rapier#./crates/rapier#g' "$tmp"/Cargo.toml
 currdir=$(pwd)
 cd "$tmp" && cargo publish
 cd "$currdir" || exit
 
 
 ### Publish the 3D version.
-sed 's#\.\./\.\./src#src#g' build/rapier_testbed3d/Cargo.toml > "$tmp"/Cargo.toml
-sed -i 's#\.\./rapier#./build/rapier#g' "$tmp"/Cargo.toml
+sed 's#\.\./\.\./src#src#g' crates/rapier_testbed3d/Cargo.toml > "$tmp"/Cargo.toml
+sed -i 's#\.\./rapier#./crates/rapier#g' "$tmp"/Cargo.toml
 cp -r LICENSE README.md "$tmp"/.
 cd "$tmp" && cargo publish
 
diff --git a/src/counters/mod.rs b/src/counters/mod.rs
index 4d4d05d..324696f 100644
--- a/src/counters/mod.rs
+++ b/src/counters/mod.rs
@@ -14,7 +14,7 @@ mod solver_counters;
 mod stages_counters;
 mod timer;
 
-/// Aggregation of all the performances counters tracked by nphysics.
+/// Aggregation of all the performances counters tracked by rapier.
 #[derive(Clone, Copy)]
 pub struct Counters {
     /// Whether thi counter is enabled or not.
@@ -34,7 +34,7 @@ pub struct Counters {
 }
 
 impl Counters {
-    /// Create a new set of counters initialized to wero.
+    /// Create a new set of counters initialized to zero.
     pub fn new(enabled: bool) -> Self {
         Counters {
             enabled,
diff --git a/src/data/coarena.rs b/src/data/coarena.rs
index 124d85a..1f01c05 100644
--- a/src/data/coarena.rs
+++ b/src/data/coarena.rs
@@ -13,6 +13,14 @@ impl<T> Coarena<T> {
         Self { data: Vec::new() }
     }
 
+    pub fn iter(&self) -> impl Iterator<Item = (Index, &T)> {
+        self.data
+            .iter()
+            .enumerate()
+            .filter(|(_, elt)| elt.0 != u32::MAX)
+            .map(|(i, elt)| (Index::from_raw_parts(i as u32, elt.0), &elt.1))
+    }
+
     /// Gets a specific element from the coarena without specifying its generation number.
     ///
     /// It is strongly encouraged to use `Coarena::get` instead of this method because this method
@@ -23,12 +31,12 @@ impl<T> Coarena<T> {
 
     /// Deletes an element for the coarena and returns its value.
     ///
-    /// We can't really remove an element from the coarena. So instead of actually removing
-    /// it, this method will reset the value to the given `removed_value`.
+    /// This method will reset the value to the given `removed_value`.
     pub fn remove(&mut self, index: Index, removed_value: T) -> Option<T> {
         let (i, g) = index.into_raw_parts();
         let data = self.data.get_mut(i as usize)?;
         if g == data.0 {
+            data.0 = u32::MAX; // invalidate the generation number.
             Some(std::mem::replace(&mut data.1, removed_value))
         } else {
             None
diff --git a/src/data/graph.rs b/src/data/graph.rs
index de958c3..2dc7fbf 100644
--- a/src/data/graph.rs
+++ b/src/data/graph.rs
@@ -644,19 +644,24 @@ impl<'a, E> Iterator for Edges<'a, E> {
 
         // For iterate_over, "both" is represented as None.
         // For reverse, "no" is represented as None.
-        let (iterate_over, reverse) = (None, Some(self.direction.opposite()));
+        let (iterate_over, _reverse) = (None, Some(self.direction.opposite()));
 
         if iterate_over.unwrap_or(Direction::Outgoing) == Direction::Outgoing {
             let i = self.next[0].index();
-            if let Some(Edge { node, weight, next }) = self.edges.get(i) {
+            if let Some(Edge {
+                node: _node,
+                weight,
+                next,
+            }) = self.edges.get(i)
+            {
                 self.next[0] = next[0];
                 return Some(EdgeReference {
                     index: EdgeIndex(i as u32),
-                    node: if reverse == Some(Direction::Outgoing) {
-                        swap_pair(*node)
-                    } else {
-                        *node
-                    },
+                    // node: if reverse == Some(Direction::Outgoing) {
+                    //     swap_pair(*node)
+                    // } else {
+                    //     *node
+                    // },
                     weight,
                 });
             }
@@ -674,11 +679,11 @@ impl<'a, E> Iterator for Edges<'a, E> {
 
                 return Some(EdgeReference {
                     index: edge_index,
-                    node: if reverse == Some(Direction::Incoming) {
-                        swap_pair(*node)
-                    } else {
-                        *node
-                    },
+                    // node: if reverse == Some(Direction::Incoming) {
+                    //     swap_pair(*node)
+                    // } else {
+                    //     *node
+                    // },
                     weight,
                 });
             }
@@ -688,10 +693,10 @@ impl<'a, E> Iterator for Edges<'a, E> {
     }
 }
 
-fn swap_pair<T>(mut x: [T; 2]) -> [T; 2] {
-    x.swap(0, 1);
-    x
-}
+// fn swap_pair<T>(mut x: [T; 2]) -> [T; 2] {
+//     x.swap(0, 1);
+//     x
+// }
 
 impl<'a, E> Clone for Edges<'a, E> {
     fn clone(&self) -> Self {
@@ -742,24 +747,11 @@ impl<N, E> IndexMut<EdgeIndex> for Graph<N, E> {
     }
 }
 
-/// A “walker” object that can be used to step through the edge list of a node.
-///
-/// Created with [`.detach()`](struct.Neighbors.html#method.detach).
-///
-/// The walker does not borrow from the graph, so it lets you step through
-/// neighbors or incident edges while also mutating graph weights, as
-/// in the following example:
-#[derive(Clone)]
-pub struct WalkNeighbors {
-    skip_start: NodeIndex,
-    next: [EdgeIndex; 2],
-}
-
 /// Reference to a `Graph` edge.
 #[derive(Debug)]
 pub struct EdgeReference<'a, E: 'a> {
     index: EdgeIndex,
-    node: [NodeIndex; 2],
+    // node: [NodeIndex; 2],
     weight: &'a E,
 }
 
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 4725319..d998eec 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -18,18 +18,6 @@ pub struct IntegrationParameters {
     /// to numerical instabilities.
     pub min_ccd_dt: Real,
 
-    /// The Error Reduction Parameter in `[0, 1]` is the proportion of
-    /// the positional error to be corrected at each time step (default: `0.2`).
-    pub erp: Real,
-    /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of
-    /// the positional error to be corrected at each time step (default: `0.2`).
-    pub joint_erp: Real,
-    /// Each cached impulse are multiplied by this coefficient in `[0, 1]`
-    /// when they are re-used to initialize the solver (default `1.0`).
-    pub warmstart_coeff: Real,
-    /// Correction factor to avoid large warmstart impulse after a strong impact (default `10.0`).
-    pub warmstart_correction_slope: Real,
-
     /// 0-1: how much of the velocity to dampen out in the constraint solver?
     /// (default `1.0`).
     pub velocity_solve_fraction: Real,
@@ -40,23 +28,21 @@ pub struct IntegrationParameters {
     /// If non-zero, you do not need the positional solver.
     /// A good non-zero value is around `0.2`.
     /// (default `0.0`).
-    pub velocity_based_erp: Real,
+    pub erp: Real,
 
-    /// Amount of penetration the engine wont attempt to correct (default: `0.005m`).
+    /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
     pub allowed_linear_error: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
     pub prediction_distance: Real,
-    /// Amount of angular drift of joint limits the engine wont
-    /// attempt to correct (default: `0.001rad`).
-    pub allowed_angular_error: Real,
-    /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`).
-    pub max_linear_correction: Real,
-    /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`).
-    pub max_angular_correction: Real,
-    /// Maximum number of iterations performed by the velocity constraints solver (default: `4`).
+    /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`).
     pub max_velocity_iterations: usize,
-    /// Maximum number of iterations performed by the position-based constraints solver (default: `1`).
-    pub max_position_iterations: usize,
+    /// Maximum number of iterations performed to solve friction constraints (default: `8`).
+    pub max_velocity_friction_iterations: usize,
+    /// Maximum number of iterations performed to remove the energy introduced by penetration corrections  (default: `1`).
+    pub max_stabilization_iterations: usize,
+    /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise,
+    /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`).
+    pub interleave_restitution_and_friction_resolution: bool,
     /// Minimum number of dynamic bodies in each active island (default: `128`).
     pub min_island_size: usize,
     /// Maximum number of substeps performed by the  solver (default: `1`).
@@ -64,46 +50,6 @@ pub struct IntegrationParameters {
 }
 
 impl IntegrationParameters {
-    /// Creates a set of integration parameters with the given values.
-    #[deprecated = "Use `IntegrationParameters { dt: 60.0, ..Default::default() }` instead"]
-    pub fn new(
-        dt: Real,
-        erp: Real,
-        joint_erp: Real,
-        warmstart_coeff: Real,
-        allowed_linear_error: Real,
-        allowed_angular_error: Real,
-        max_linear_correction: Real,
-        max_angular_correction: Real,
-        prediction_distance: Real,
-        max_velocity_iterations: usize,
-        max_position_iterations: usize,
-        max_ccd_substeps: usize,
-    ) -> Self {
-        IntegrationParameters {
-            dt,
-            erp,
-            joint_erp,
-            warmstart_coeff,
-            allowed_linear_error,
-            allowed_angular_error,
-            max_linear_correction,
-            max_angular_correction,
-            prediction_distance,
-            max_velocity_iterations,
-            max_position_iterations,
-            max_ccd_substeps,
-            ..Default::default()
-        }
-    }
-
-    /// The current time-stepping length.
-    #[inline(always)]
-    #[deprecated = "You can just read the `IntegrationParams::dt` value directly"]
-    pub fn dt(&self) -> Real {
-        self.dt
-    }
-
     /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz).
     ///
     /// This is zero if `self.dt` is zero.
@@ -136,10 +82,10 @@ impl IntegrationParameters {
         }
     }
 
-    /// Convenience: `velocity_based_erp / dt`
+    /// Convenience: `erp / dt`
     #[inline]
-    pub(crate) fn velocity_based_erp_inv_dt(&self) -> Real {
-        self.velocity_based_erp * self.inv_dt()
+    pub(crate) fn erp_inv_dt(&self) -> Real {
+        self.erp * self.inv_dt()
     }
 }
 
@@ -148,20 +94,14 @@ impl Default for IntegrationParameters {
         Self {
             dt: 1.0 / 60.0,
             min_ccd_dt: 1.0 / 60.0 / 100.0,
-            //        multithreading_enabled:             true,
-            erp: 0.2,
-            joint_erp: 0.2,
             velocity_solve_fraction: 1.0,
-            velocity_based_erp: 0.0,
-            warmstart_coeff: 1.0,
-            warmstart_correction_slope: 10.0,
-            allowed_linear_error: 0.005,
+            erp: 0.8,
+            allowed_linear_error: 0.001, // 0.005
             prediction_distance: 0.002,
-            allowed_angular_error: 0.001,
-            max_linear_correction: 0.2,
-            max_angular_correction: 0.2,
             max_velocity_iterations: 4,
-            max_position_iterations: 1,
+            max_velocity_friction_iterations: 8,
+            max_stabilization_iterations: 1,
+            interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability.
             // FIXME: what is the optimal value for min_island_size?
             // It should not be too big so that we don't end up with
             // huge islands that don't fit in cache.
diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index 10ac0f8..55b47c9 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -1,7 +1,7 @@
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 use crate::dynamics::{
-    JointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, RigidBodyIds,
-    RigidBodyType, RigidBodyVelocity,
+    ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle,
+    RigidBodyIds, RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::{ColliderParent, NarrowPhase};
 use crate::math::Real;
@@ -175,7 +175,8 @@ impl IslandManager {
         bodies: &mut Bodies,
         colliders: &Colliders,
         narrow_phase: &NarrowPhase,
-        joints: &JointSet,
+        impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
         min_island_size: usize,
     ) where
         Bodies: ComponentSetMut<RigidBodyIds>
@@ -302,11 +303,15 @@ impl IslandManager {
             // in contact or joined with this collider.
             push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack);
 
-            for inter in joints.joints_with(handle) {
+            for inter in impulse_joints.joints_with(handle) {
                 let other = crate::utils::select_other((inter.0, inter.1), handle);
                 self.stack.push(other);
             }
 
+            for other in multibody_joints.attached_bodies(handle) {
+                self.stack.push(other);
+            }
+
             bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
                 activation.wake_up(false);
             });
diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
deleted file mode 100644
index 0b626c0..0000000
--- a/src/dynamics/joint/ball_joint.rs
+++ /dev/null
@@ -1,148 +0,0 @@
-use crate::dynamics::SpringModel;
-use crate::math::{Point, Real, Rotation, UnitVector, Vector};
-
-#[derive(Copy, Clone, PartialEq)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// A joint that removes all relative linear motion between a pair of points on two bodies.
-pub struct BallJoint {
-    /// Where the ball joint is attached on the first body, expressed in the first body local frame.
-    pub local_anchor1: Point<Real>,
-    /// Where the ball joint is attached on the second body, expressed in the second body local frame.
-    pub local_anchor2: Point<Real>,
-    /// The impulse applied by this joint on the first body.
-    ///
-    /// The impulse applied to the second body is given by `-impulse`.
-    pub impulse: Vector<Real>,
-
-    /// The target relative angular velocity the motor will attempt to reach.
-    #[cfg(feature = "dim2")]
-    pub motor_target_vel: Real,
-    /// The target relative angular velocity the motor will attempt to reach.
-    #[cfg(feature = "dim3")]
-    pub motor_target_vel: Vector<Real>,
-    /// The target angular position of this joint, expressed as an axis-angle.
-    pub motor_target_pos: Rotation<Real>,
-    /// The motor's stiffness.
-    /// See the documentation of `SpringModel` for more information on this parameter.
-    pub motor_stiffness: Real,
-    /// The motor's damping.
-    /// See the documentation of `SpringModel` for more information on this parameter.
-    pub motor_damping: Real,
-    /// The maximal impulse the motor is able to deliver.
-    pub motor_max_impulse: Real,
-    /// The angular impulse applied by the motor.
-    #[cfg(feature = "dim2")]
-    pub motor_impulse: Real,
-    /// The angular impulse applied by the motor.
-    #[cfg(feature = "dim3")]
-    pub motor_impulse: Vector<Real>,
-    /// The spring-like model used by the motor to reach the target velocity and .
-    pub motor_model: SpringModel,
-
-    /// Are the limits enabled for this joint?
-    pub limits_enabled: bool,
-    /// The axis of the limit cone for this joint, if the local-space of the first body.
-    pub limits_local_axis1: UnitVector<Real>,
-    /// The axis of the limit cone for this joint, if the local-space of the first body.
-    pub limits_local_axis2: UnitVector<Real>,
-    /// The maximum angle allowed between the two limit axes in world-space.
-    pub limits_angle: Real,
-    /// The impulse applied to enforce joint limits.
-    pub limits_impulse: Real,
-}
-
-impl BallJoint {
-    /// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies.
-    pub fn new(local_anchor1: Point<Real>, local_anchor2: Point<Real>) -> Self {
-        Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
-    }
-
-    pub(crate) fn with_impulse(
-        local_anchor1: Point<Real>,
-        local_anchor2: Point<Real>,
-        impulse: Vector<Real>,
-    ) -> Self {
-        Self {
-            local_anchor1,
-            local_anchor2,
-            impulse,
-            motor_target_vel: na::zero(),
-            motor_target_pos: Rotation::identity(),
-            motor_stiffness: 0.0,
-            motor_damping: 0.0,
-            motor_impulse: na::zero(),
-            motor_max_impulse: Real::MAX,
-            motor_model: SpringModel::default(),
-            limits_enabled: false,
-            limits_local_axis1: Vector::x_axis(),
-            limits_local_axis2: Vector::x_axis(),
-            limits_angle: Real::MAX,
-            limits_impulse: 0.0,
-        }
-    }
-
-    /// Can a SIMD constraint be used for resolving this joint?
-    pub fn supports_simd_constraints(&self) -> bool {
-        // SIMD ball constraints don't support motors and limits right now.
-        !self.limits_enabled
-            && (self.motor_max_impulse == 0.0
-                || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
-    }
-
-    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
-    pub fn configure_motor_model(&mut self, model: SpringModel) {
-        self.motor_model = model;
-    }
-
-    /// Sets the target velocity and velocity correction factor this motor.
-    #[cfg(feature = "dim2")]
-    pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
-        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
-    }
-
-    /// Sets the target velocity and velocity correction factor this motor.
-    #[cfg(feature = "dim3")]
-    pub fn configure_motor_velocity(&mut self, target_vel: Vector<Real>, factor: Real) {
-        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
-    }
-
-    /// Sets the target orientation this motor needs to reach.
-    pub fn configure_motor_position(
-        &mut self,
-        target_pos: Rotation<Real>,
-        stiffness: Real,
-        damping: Real,
-    ) {
-        self.configure_motor(target_pos, na::zero(), stiffness, damping)
-    }
-
-    /// Sets the target orientation this motor needs to reach.
-    #[cfg(feature = "dim2")]
-    pub fn configure_motor(
-        &mut self,
-        target_pos: Rotation<Real>,
-        target_vel: Real,
-        stiffness: Real,
-        damping: Real,
-    ) {
-        self.motor_target_vel = target_vel;
-        self.motor_target_pos = target_pos;
-        self.motor_stiffness = stiffness;
-        self.motor_damping = damping;
-    }
-
-    /// Configure both the target orientation and target velocity of the motor.
-    #[cfg(feature = "dim3")]
-    pub fn configure_motor(
-        &mut self,
-        target_pos: Rotation<Real>,
-        target_vel: Vector<Real>,
-        stiffness: Real,
-        damping: Real,
-    ) {
-        self.motor_target_vel = target_vel;
-        self.motor_target_pos = target_pos;
-        self.motor_stiffness = stiffness;
-        self.motor_damping = damping;
-    }
-}
diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs
index 6424750..10c4d7e 100644
--- a/src/dynamics/joint/fixed_joint.rs
+++ b/src/dynamics/joint/fixed_joint.rs
@@ -1,38 +1,55 @@
-use crate::math::{Isometry, Real, SpacialVector};
+use crate::dynamics::{JointAxesMask, JointData};
+use crate::math::{Isometry, Point, Real};
 
-#[derive(Copy, Clone, PartialEq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// A joint that prevents all relative movement between two bodies.
-///
-/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
+#[derive(Copy, Clone, Debug, PartialEq)]
 pub struct FixedJoint {
-    /// The frame of reference for the first body affected by this joint, expressed in the local frame
-    /// of the first body.
-    pub local_frame1: Isometry<Real>,
-    /// The frame of reference for the second body affected by this joint, expressed in the local frame
-    /// of the first body.
-    pub local_frame2: Isometry<Real>,
-    /// The impulse applied to the first body affected by this joint.
-    ///
-    /// The impulse applied to the second body affected by this joint is given by `-impulse`.
-    /// This combines both linear and angular impulses:
-    /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
-    /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
-    pub impulse: SpacialVector<Real>,
+    data: JointData,
 }
 
 impl FixedJoint {
-    /// Creates a new fixed joint from the frames of reference of both bodies.
-    pub fn new(local_frame1: Isometry<Real>, local_frame2: Isometry<Real>) -> Self {
-        Self {
-            local_frame1,
-            local_frame2,
-            impulse: SpacialVector::zeros(),
-        }
+    pub fn new() -> Self {
+        #[cfg(feature = "dim2")]
+        let mask = JointAxesMask::X | JointAxesMask::Y | JointAxesMask::ANG_X;
+        #[cfg(feature = "dim3")]
+        let mask = JointAxesMask::X
+            | JointAxesMask::Y
+            | JointAxesMask::Z
+            | JointAxesMask::ANG_X
+            | JointAxesMask::ANG_Y
+            | JointAxesMask::ANG_Z;
+
+        let data = JointData::default().lock_axes(mask);
+        Self { data }
     }
 
-    /// Can a SIMD constraint be used for resolving this joint?
-    pub fn supports_simd_constraints(&self) -> bool {
-        true
+    #[must_use]
+    pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
+        self.data = self.data.local_frame1(local_frame);
+        self
+    }
+
+    #[must_use]
+    pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
+        self.data = self.data.local_frame2(local_frame);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+        self.data = self.data.local_anchor1(anchor1);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+        self.data = self.data.local_anchor2(anchor2);
+        self
+    }
+}
+
+impl Into<JointData> for FixedJoint {
+    fn into(self) -> JointData {
+        self.data
     }
 }
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
deleted file mode 100644
index 78f1e84..0000000
--- a/src/dynamics/joint/generic_joint.rs
+++ /dev/null
@@ -1,144 +0,0 @@
-use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint};
-use crate::math::{Isometry, Real, SpacialVector};
-use crate::na::{Rotation3, UnitQuaternion};
-
-#[derive(Copy, Clone, Debug, PartialEq)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// A joint that prevents all relative movement between two bodies.
-///
-/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
-pub struct GenericJoint {
-    /// The frame of reference for the first body affected by this joint, expressed in the local frame
-    /// of the first body.
-    pub local_anchor1: Isometry<Real>,
-    /// The frame of reference for the second body affected by this joint, expressed in the local frame
-    /// of the first body.
-    pub local_anchor2: Isometry<Real>,
-    /// The impulse applied to the first body affected by this joint.
-    ///
-    /// The impulse applied to the second body affected by this joint is given by `-impulse`.
-    /// This combines both linear and angular impulses:
-    /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
-    /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
-    pub impulse: SpacialVector<Real>,
-
-    pub min_position: SpacialVector<Real>,
-    pub max_position: SpacialVector<Real>,
-    pub min_velocity: SpacialVector<Real>,
-    pub max_velocity: SpacialVector<Real>,
-    /// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0
-    pub min_impulse: SpacialVector<Real>,
-    /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0
-    pub max_impulse: SpacialVector<Real>,
-    /// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0
-    pub min_pos_impulse: SpacialVector<Real>,
-    /// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0
-    pub max_pos_impulse: SpacialVector<Real>,
-}
-
-impl GenericJoint {
-    /// Creates a new fixed joint from the frames of reference of both bodies.
-    pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self {
-        Self {
-            local_anchor1,
-            local_anchor2,
-            impulse: SpacialVector::zeros(),
-            min_position: SpacialVector::zeros(),
-            max_position: SpacialVector::zeros(),
-            min_velocity: SpacialVector::zeros(),
-            max_velocity: SpacialVector::zeros(),
-            min_impulse: SpacialVector::repeat(-Real::MAX),
-            max_impulse: SpacialVector::repeat(Real::MAX),
-            min_pos_impulse: SpacialVector::repeat(-Real::MAX),
-            max_pos_impulse: SpacialVector::repeat(Real::MAX),
-        }
-    }
-
-    pub fn set_dof_vel(&mut self, dof: u8, target_vel: Real, max_force: Real) {
-        self.min_velocity[dof as usize] = target_vel;
-        self.max_velocity[dof as usize] = target_vel;
-        self.min_impulse[dof as usize] = -max_force;
-        self.max_impulse[dof as usize] = max_force;
-    }
-
-    pub fn free_dof(&mut self, dof: u8) {
-        self.min_position[dof as usize] = -Real::MAX;
-        self.max_position[dof as usize] = Real::MAX;
-        self.min_velocity[dof as usize] = -Real::MAX;
-        self.max_velocity[dof as usize] = Real::MAX;
-        self.min_impulse[dof as usize] = 0.0;
-        self.max_impulse[dof as usize] = 0.0;
-        self.min_pos_impulse[dof as usize] = 0.0;
-        self.max_pos_impulse[dof as usize] = 0.0;
-    }
-
-    pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) {
-        self.min_position[dof as usize] = min;
-        self.max_position[dof as usize] = max;
-    }
-}
-
-impl From<RevoluteJoint> for GenericJoint {
-    fn from(joint: RevoluteJoint) -> Self {
-        let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
-        let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
-        let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
-        let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
-        let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
-        let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
-
-        let mut result = Self::new(local_anchor1, local_anchor2);
-        result.free_dof(3);
-
-        if joint.motor_damping != 0.0 {
-            result.set_dof_vel(3, joint.motor_target_vel, joint.motor_max_impulse);
-        }
-
-        result.impulse[0] = joint.impulse[0];
-        result.impulse[1] = joint.impulse[1];
-        result.impulse[2] = joint.impulse[2];
-        result.impulse[3] = joint.motor_impulse;
-        result.impulse[4] = joint.impulse[3];
-        result.impulse[5] = joint.impulse[4];
-
-        result
-    }
-}
-
-impl From<BallJoint> for GenericJoint {
-    fn from(joint: BallJoint) -> Self {
-        let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero());
-        let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero());
-
-        let mut result = Self::new(local_anchor1, local_anchor2);
-        result.impulse[0] = joint.impulse[0];
-        result.impulse[1] = joint.impulse[1];
-        result.impulse[2] = joint.impulse[2];
-        result.free_dof(3);
-        result.free_dof(4);
-        result.free_dof(5);
-        result
-    }
-}
-
-impl From<PrismaticJoint> for GenericJoint {
-    fn from(joint: PrismaticJoint) -> Self {
-        let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
-        let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
-        let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
-        let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
-        let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
-        let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
-
-        let mut result = Self::new(local_anchor1, local_anchor2);
-        result.free_dof(0);
-        result.set_dof_limits(0, joint.limits[0], joint.limits[1]);
-        result
-    }
-}
-
-impl From<FixedJoint> for GenericJoint {
-    fn from(joint: FixedJoint) -> Self {
-        Self::new(joint.local_anchor1, joint.local_anchor2)
-    }
-}
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint.rs b/src/dynamics/joint/impulse_joint/impulse_joint.rs
new file mode 100644
index 0000000..a12c533
--- /dev/null
+++ b/src/dynamics/joint/impulse_joint/impulse_joint.rs
@@ -0,0 +1,20 @@
+use crate::dynamics::{JointData, JointHandle, RigidBodyHandle};
+use crate::math::{Real, SpacialVector};
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug, PartialEq)]
+/// A joint attached to two bodies.
+pub struct ImpulseJoint {
+    /// Handle to the first body attached to this joint.
+    pub body1: RigidBodyHandle,
+    /// Handle to the second body attached to this joint.
+    pub body2: RigidBodyHandle,
+
+    pub data: JointData,
+    pub impulses: SpacialVector<Real>,
+
+    // A joint needs to know its handle to simplify its removal.
+    pub(crate) handle: JointHandle,
+    #[cfg(feature = "parallel")]
+    pub(crate) constraint_index: usize,
+}
diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
similarity index 86%
rename from src/dynamics/joint/joint_set.rs
rename to src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index aba2aa8..183b668 100644
--- a/src/dynamics/joint/joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -1,10 +1,10 @@
-use super::Joint;
+use super::ImpulseJoint;
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
 
 use crate::data::arena::Arena;
 use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut};
 use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
-use crate::dynamics::{JointParams, RigidBodyHandle};
+use crate::dynamics::{JointData, RigidBodyHandle};
 
 /// The unique identifier of a joint added to the joint set.
 /// The unique identifier of a collider added to a collider set.
@@ -34,19 +34,19 @@ impl JointHandle {
 }
 
 pub(crate) type JointIndex = usize;
-pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
+pub(crate) type JointGraphEdge = crate::data::graph::Edge<ImpulseJoint>;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone, Default)]
-/// A set of joints that can be handled by a physics `World`.
-pub struct JointSet {
+/// A set of impulse_joints that can be handled by a physics `World`.
+pub struct ImpulseJointSet {
     rb_graph_ids: Coarena<RigidBodyGraphIndex>,
     joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
-    joint_graph: InteractionGraph<RigidBodyHandle, Joint>,
+    joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>,
 }
 
-impl JointSet {
-    /// Creates a new empty set of joints.
+impl ImpulseJointSet {
+    /// Creates a new empty set of impulse_joints.
     pub fn new() -> Self {
         Self {
             rb_graph_ids: Coarena::new(),
@@ -55,26 +55,26 @@ impl JointSet {
         }
     }
 
-    /// The number of joints on this set.
+    /// The number of impulse_joints on this set.
     pub fn len(&self) -> usize {
         self.joint_graph.graph.edges.len()
     }
 
-    /// `true` if there are no joints in this set.
+    /// `true` if there are no impulse_joints in this set.
     pub fn is_empty(&self) -> bool {
         self.joint_graph.graph.edges.is_empty()
     }
 
-    /// Retrieve the joint graph where edges are joints and nodes are rigid body handles.
-    pub fn joint_graph(&self) -> &InteractionGraph<RigidBodyHandle, Joint> {
+    /// Retrieve the joint graph where edges are impulse_joints and nodes are rigid body handles.
+    pub fn joint_graph(&self) -> &InteractionGraph<RigidBodyHandle, ImpulseJoint> {
         &self.joint_graph
     }
 
-    /// Iterates through all the joitns attached to the given rigid-body.
+    /// Iterates through all the impulse_joints attached to the given rigid-body.
     pub fn joints_with<'a>(
         &'a self,
         body: RigidBodyHandle,
-    ) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyHandle, &'a Joint)> {
+    ) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyHandle, &'a ImpulseJoint)> {
         self.rb_graph_ids
             .get(body.0)
             .into_iter()
@@ -87,13 +87,13 @@ impl JointSet {
     }
 
     /// Gets the joint with the given handle.
-    pub fn get(&self, handle: JointHandle) -> Option<&Joint> {
+    pub fn get(&self, handle: JointHandle) -> Option<&ImpulseJoint> {
         let id = self.joint_ids.get(handle.0)?;
         self.joint_graph.graph.edge_weight(*id)
     }
 
     /// Gets a mutable reference to the joint with the given handle.
-    pub fn get_mut(&mut self, handle: JointHandle) -> Option<&mut Joint> {
+    pub fn get_mut(&mut self, handle: JointHandle) -> Option<&mut ImpulseJoint> {
         let id = self.joint_ids.get(handle.0)?;
         self.joint_graph.graph.edge_weight_mut(*id)
     }
@@ -107,7 +107,7 @@ impl JointSet {
     ///
     /// Using this is discouraged in favor of `self.get(handle)` which does not
     /// suffer form the ABA problem.
-    pub fn get_unknown_gen(&self, i: u32) -> Option<(&Joint, JointHandle)> {
+    pub fn get_unknown_gen(&self, i: u32) -> Option<(&ImpulseJoint, JointHandle)> {
         let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
         Some((
             self.joint_graph.graph.edge_weight(*id)?,
@@ -124,7 +124,7 @@ impl JointSet {
     ///
     /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
     /// suffer form the ABA problem.
-    pub fn get_unknown_gen_mut(&mut self, i: u32) -> Option<(&mut Joint, JointHandle)> {
+    pub fn get_unknown_gen_mut(&mut self, i: u32) -> Option<(&mut ImpulseJoint, JointHandle)> {
         let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
         Some((
             self.joint_graph.graph.edge_weight_mut(*id)?,
@@ -133,7 +133,7 @@ impl JointSet {
     }
 
     /// Iterates through all the joint on this set.
-    pub fn iter(&self) -> impl Iterator<Item = (JointHandle, &Joint)> {
+    pub fn iter(&self) -> impl Iterator<Item = (JointHandle, &ImpulseJoint)> {
         self.joint_graph
             .graph
             .edges
@@ -142,7 +142,7 @@ impl JointSet {
     }
 
     /// Iterates mutably through all the joint on this set.
-    pub fn iter_mut(&mut self) -> impl Iterator<Item = (JointHandle, &mut Joint)> {
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = (JointHandle, &mut ImpulseJoint)> {
         self.joint_graph
             .graph
             .edges
@@ -150,8 +150,8 @@ impl JointSet {
             .map(|e| (e.weight.handle, &mut e.weight))
     }
 
-    // /// The set of joints as an array.
-    // pub(crate) fn joints(&self) -> &[JointGraphEdge] {
+    // /// The set of impulse_joints as an array.
+    // pub(crate) fn impulse_joints(&self) -> &[JointGraphEdge] {
     //     // self.joint_graph
     //     //     .graph
     //     //     .edges
@@ -170,25 +170,22 @@ impl JointSet {
     }
 
     /// Inserts a new joint into this set and retrieve its handle.
-    pub fn insert<J>(
+    pub fn insert(
         &mut self,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
-        joint_params: J,
-    ) -> JointHandle
-    where
-        J: Into<JointParams>,
-    {
+        data: impl Into<JointData>,
+    ) -> JointHandle {
+        let data = data.into();
         let handle = self.joint_ids.insert(0.into());
-        let joint = Joint {
+        let joint = ImpulseJoint {
             body1,
             body2,
+            data,
+            impulses: na::zero(),
             handle: JointHandle(handle),
             #[cfg(feature = "parallel")]
             constraint_index: 0,
-            #[cfg(feature = "parallel")]
-            position_constraint_index: 0,
-            params: joint_params.into(),
         };
 
         let default_id = InteractionGraph::<(), ()>::invalid_graph_index();
@@ -201,12 +198,12 @@ impl JointSet {
 
         // NOTE: the body won't have a graph index if it does not
         // have any joint attached.
-        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index1) {
+        if !InteractionGraph::<RigidBodyHandle, ImpulseJoint>::is_graph_index_valid(graph_index1) {
             graph_index1 = self.joint_graph.graph.add_node(joint.body1);
             self.rb_graph_ids.insert(joint.body1.0, graph_index1);
         }
 
-        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index2) {
+        if !InteractionGraph::<RigidBodyHandle, ImpulseJoint>::is_graph_index_valid(graph_index2) {
             graph_index2 = self.joint_graph.graph.add_node(joint.body2);
             self.rb_graph_ids.insert(joint.body2.0, graph_index2);
         }
@@ -215,7 +212,7 @@ impl JointSet {
         JointHandle(handle)
     }
 
-    /// Retrieve all the joints happening between two active bodies.
+    /// Retrieve all the impulse_joints happening between two active bodies.
     // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
     pub(crate) fn select_active_interactions<Bodies>(
         &self,
@@ -271,7 +268,7 @@ impl JointSet {
         islands: &mut IslandManager,
         bodies: &mut Bodies,
         wake_up: bool,
-    ) -> Option<Joint>
+    ) -> Option<ImpulseJoint>
     where
         Bodies: ComponentSetMut<RigidBodyActivation>
             + ComponentSet<RigidBodyType>
@@ -299,11 +296,11 @@ impl JointSet {
         removed_joint
     }
 
-    /// Deletes all the joints attached to the given rigid-body.
+    /// Deletes all the impulse_joints attached to the given rigid-body.
     ///
     /// The provided rigid-body handle is not required to identify a rigid-body that
     /// is still contained by the `bodies` component set.
-    /// Returns the (now invalid) handles of the removed joints.
+    /// Returns the (now invalid) handles of the removed impulse_joints.
     pub fn remove_joints_attached_to_rigid_body<Bodies>(
         &mut self,
         handle: RigidBodyHandle,
diff --git a/src/dynamics/joint/impulse_joint/mod.rs b/src/dynamics/joint/impulse_joint/mod.rs
new file mode 100644
index 0000000..2afe078
--- /dev/null
+++ b/src/dynamics/joint/impulse_joint/mod.rs
@@ -0,0 +1,6 @@
+pub use self::impulse_joint::ImpulseJoint;
+pub use self::impulse_joint_set::{ImpulseJointSet, JointHandle};
+pub(crate) use self::impulse_joint_set::{JointGraphEdge, JointIndex};
+
+mod impulse_joint;
+mod impulse_joint_set;
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
deleted file mode 100644
index 0c2c864..0000000
--- a/src/dynamics/joint/joint.rs
+++ /dev/null
@@ -1,143 +0,0 @@
-#[cfg(feature = "dim3")]
-use crate::dynamics::RevoluteJoint;
-use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
-
-#[derive(Copy, Clone, PartialEq)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// An enum grouping all possible types of joints.
-pub enum JointParams {
-    /// A Ball joint that removes all relative linear degrees of freedom between the affected bodies.
-    BallJoint(BallJoint),
-    /// A fixed joint that removes all relative degrees of freedom between the affected bodies.
-    FixedJoint(FixedJoint),
-    /// A prismatic joint that removes all degrees of degrees of freedom between the affected
-    /// bodies except for the translation along one axis.
-    PrismaticJoint(PrismaticJoint),
-    #[cfg(feature = "dim3")]
-    /// A revolute joint that removes all degrees of degrees of freedom between the affected
-    /// bodies except for the translation along one axis.
-    RevoluteJoint(RevoluteJoint),
-    // GenericJoint(GenericJoint),
-}
-
-impl JointParams {
-    /// An integer identifier for each type of joint.
-    pub fn type_id(&self) -> usize {
-        match self {
-            JointParams::BallJoint(_) => 0,
-            JointParams::FixedJoint(_) => 1,
-            JointParams::PrismaticJoint(_) => 2,
-            // JointParams::GenericJoint(_) => 3,
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(_) => 4,
-        }
-    }
-
-    /// Gets a reference to the underlying ball joint, if `self` is one.
-    pub fn as_ball_joint(&self) -> Option<&BallJoint> {
-        if let JointParams::BallJoint(j) = self {
-            Some(j)
-        } else {
-            None
-        }
-    }
-
-    /// Gets a reference to the underlying fixed joint, if `self` is one.
-    pub fn as_fixed_joint(&self) -> Option<&FixedJoint> {
-        if let JointParams::FixedJoint(j) = self {
-            Some(j)
-        } else {
-            None
-        }
-    }
-
-    // /// Gets a reference to the underlying generic joint, if `self` is one.
-    // pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
-    //     if let JointParams::GenericJoint(j) = self {
-    //         Some(j)
-    //     } else {
-    //         None
-    //     }
-    // }
-
-    /// Gets a reference to the underlying prismatic joint, if `self` is one.
-    pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
-        if let JointParams::PrismaticJoint(j) = self {
-            Some(j)
-        } else {
-            None
-        }
-    }
-
-    /// Gets a reference to the underlying revolute joint, if `self` is one.
-    #[cfg(feature = "dim3")]
-    pub fn as_revolute_joint(&self) -> Option<&RevoluteJoint> {
-        if let JointParams::RevoluteJoint(j) = self {
-            Some(j)
-        } else {
-            None
-        }
-    }
-}
-
-impl From<BallJoint> for JointParams {
-    fn from(j: BallJoint) -> Self {
-        JointParams::BallJoint(j)
-    }
-}
-
-impl From<FixedJoint> for JointParams {
-    fn from(j: FixedJoint) -> Self {
-        JointParams::FixedJoint(j)
-    }
-}
-
-// impl From<GenericJoint> for JointParams {
-//     fn from(j: GenericJoint) -> Self {
-//         JointParams::GenericJoint(j)
-//     }
-// }
-
-#[cfg(feature = "dim3")]
-impl From<RevoluteJoint> for JointParams {
-    fn from(j: RevoluteJoint) -> Self {
-        JointParams::RevoluteJoint(j)
-    }
-}
-
-impl From<PrismaticJoint> for JointParams {
-    fn from(j: PrismaticJoint) -> Self {
-        JointParams::PrismaticJoint(j)
-    }
-}
-
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[derive(Clone)]
-/// A joint attached to two bodies.
-pub struct Joint {
-    /// Handle to the first body attached to this joint.
-    pub body1: RigidBodyHandle,
-    /// Handle to the second body attached to this joint.
-    pub body2: RigidBodyHandle,
-    // A joint needs to know its handle to simplify its removal.
-    pub(crate) handle: JointHandle,
-    #[cfg(feature = "parallel")]
-    pub(crate) constraint_index: usize,
-    #[cfg(feature = "parallel")]
-    pub(crate) position_constraint_index: usize,
-    /// The joint geometric parameters and impulse.
-    pub params: JointParams,
-}
-
-impl Joint {
-    /// Can this joint use SIMD-accelerated constraint formulations?
-    pub fn supports_simd_constraints(&self) -> bool {
-        match &self.params {
-            JointParams::PrismaticJoint(joint) => joint.supports_simd_constraints(),
-            JointParams::FixedJoint(joint) => joint.supports_simd_constraints(),
-            JointParams::BallJoint(joint) => joint.supports_simd_constraints(),
-            #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(),
-        }
-    }
-}
diff --git a/src/dynamics/joint/joint_data.rs b/src/dynamics/joint/joint_data.rs
new file mode 100644
index 0000000..35d832d
--- /dev/null
+++ b/src/dynamics/joint/joint_data.rs
@@ -0,0 +1,270 @@
+use crate::dynamics::solver::MotorParameters;
+use crate::dynamics::MotorModel;
+use crate::math::{Isometry, Point, Real, Rotation, UnitVector, SPATIAL_DIM};
+use crate::utils::WBasis;
+
+#[cfg(feature = "dim3")]
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    pub struct JointAxesMask: u8 {
+        const FREE = 0;
+        const X = 1 << 0;
+        const Y = 1 << 1;
+        const Z = 1 << 2;
+        const ANG_X = 1 << 3;
+        const ANG_Y = 1 << 4;
+        const ANG_Z = 1 << 5;
+    }
+}
+
+#[cfg(feature = "dim2")]
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    pub struct JointAxesMask: u8 {
+        const FREE = 0;
+        const X = 1 << 0;
+        const Y = 1 << 1;
+        const ANG_X = 1 << 2;
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub enum JointAxis {
+    X = 0,
+    Y,
+    #[cfg(feature = "dim3")]
+    Z,
+    AngX,
+    #[cfg(feature = "dim3")]
+    AngY,
+    #[cfg(feature = "dim3")]
+    AngZ,
+}
+
+impl From<JointAxis> for JointAxesMask {
+    fn from(axis: JointAxis) -> Self {
+        JointAxesMask::from_bits(1 << axis as usize).unwrap()
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct JointLimits {
+    pub min: Real,
+    pub max: Real,
+    pub impulse: Real,
+}
+
+impl Default for JointLimits {
+    fn default() -> Self {
+        Self {
+            min: -Real::MAX,
+            max: Real::MAX,
+            impulse: 0.0,
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct JointMotor {
+    pub target_vel: Real,
+    pub target_pos: Real,
+    pub stiffness: Real,
+    pub damping: Real,
+    pub max_impulse: Real,
+    pub impulse: Real,
+    pub model: MotorModel,
+}
+
+impl Default for JointMotor {
+    fn default() -> Self {
+        Self {
+            target_pos: 0.0,
+            target_vel: 0.0,
+            stiffness: 0.0,
+            damping: 0.0,
+            max_impulse: Real::MAX,
+            impulse: 0.0,
+            model: MotorModel::VelocityBased,
+        }
+    }
+}
+
+impl JointMotor {
+    pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters<Real> {
+        let (stiffness, damping, gamma, _keep_lhs) =
+            self.model
+                .combine_coefficients(dt, self.stiffness, self.damping);
+        MotorParameters {
+            stiffness,
+            damping,
+            gamma,
+            // keep_lhs,
+            target_pos: self.target_pos,
+            target_vel: self.target_vel,
+            max_impulse: self.max_impulse,
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct JointData {
+    pub local_frame1: Isometry<Real>,
+    pub local_frame2: Isometry<Real>,
+    pub locked_axes: JointAxesMask,
+    pub limit_axes: JointAxesMask,
+    pub motor_axes: JointAxesMask,
+    pub limits: [JointLimits; SPATIAL_DIM],
+    pub motors: [JointMotor; SPATIAL_DIM],
+}
+
+impl Default for JointData {
+    fn default() -> Self {
+        Self {
+            local_frame1: Isometry::identity(),
+            local_frame2: Isometry::identity(),
+            locked_axes: JointAxesMask::FREE,
+            limit_axes: JointAxesMask::FREE,
+            motor_axes: JointAxesMask::FREE,
+            limits: [JointLimits::default(); SPATIAL_DIM],
+            motors: [JointMotor::default(); SPATIAL_DIM],
+        }
+    }
+}
+
+impl JointData {
+    #[must_use]
+    pub fn new(locked_axes: JointAxesMask) -> Self {
+        Self::default().lock_axes(locked_axes)
+    }
+
+    /// Can this joint use SIMD-accelerated constraint formulations?
+    pub fn supports_simd_constraints(&self) -> bool {
+        self.limit_axes.is_empty() && self.motor_axes.is_empty()
+    }
+
+    #[must_use]
+    pub fn lock_axes(mut self, axes: JointAxesMask) -> Self {
+        self.locked_axes |= axes;
+        self
+    }
+
+    fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
+        let basis = axis.orthonormal_basis();
+
+        #[cfg(feature = "dim2")]
+        {
+            use na::{Matrix2, Rotation2, UnitComplex};
+            let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]);
+            let rotmat = Rotation2::from_matrix_unchecked(mat);
+            UnitComplex::from_rotation_matrix(&rotmat)
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            use na::{Matrix3, Rotation3, UnitQuaternion};
+            let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]);
+            let rotmat = Rotation3::from_matrix_unchecked(mat);
+            UnitQuaternion::from_rotation_matrix(&rotmat)
+        }
+    }
+
+    #[must_use]
+    pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
+        self.local_frame1 = local_frame;
+        self
+    }
+
+    #[must_use]
+    pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
+        self.local_frame2 = local_frame;
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
+        self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
+        self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+        self.local_frame1.translation.vector = anchor1.coords;
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+        self.local_frame2.translation.vector = anchor2.coords;
+        self
+    }
+
+    #[must_use]
+    pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
+        let i = axis as usize;
+        self.limit_axes |= axis.into();
+        self.limits[i].min = limits[0];
+        self.limits[i].max = limits[1];
+        self
+    }
+
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
+        self.motors[axis as usize].model = model;
+        self
+    }
+
+    /// Sets the target velocity this motor needs to reach.
+    pub fn motor_velocity(self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
+        self.motor_axis(
+            axis,
+            self.motors[axis as usize].target_pos,
+            target_vel,
+            0.0,
+            factor,
+        )
+    }
+
+    /// Sets the target angle this motor needs to reach.
+    pub fn motor_position(
+        self,
+        axis: JointAxis,
+        target_pos: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> Self {
+        self.motor_axis(axis, target_pos, 0.0, stiffness, damping)
+    }
+
+    /// Configure both the target angle and target velocity of the motor.
+    pub fn motor_axis(
+        mut self,
+        axis: JointAxis,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> Self {
+        self.motor_axes |= axis.into();
+        let i = axis as usize;
+        self.motors[i].target_vel = target_vel;
+        self.motors[i].target_pos = target_pos;
+        self.motors[i].stiffness = stiffness;
+        self.motors[i].damping = damping;
+        self
+    }
+
+    pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self {
+        self.motors[axis as usize].max_impulse = max_impulse;
+        self
+    }
+}
diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs
index 72a7483..6594b83 100644
--- a/src/dynamics/joint/mod.rs
+++ b/src/dynamics/joint/mod.rs
@@ -1,20 +1,21 @@
-pub use self::ball_joint::BallJoint;
 pub use self::fixed_joint::FixedJoint;
-// pub use self::generic_joint::GenericJoint;
-pub use self::joint::{Joint, JointParams};
-pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
-pub use self::joint_set::{JointHandle, JointSet};
+pub use self::impulse_joint::*;
+pub use self::joint_data::*;
+pub use self::motor_model::MotorModel;
+pub use self::multibody_joint::*;
 pub use self::prismatic_joint::PrismaticJoint;
-#[cfg(feature = "dim3")]
 pub use self::revolute_joint::RevoluteJoint;
-pub use self::spring_model::SpringModel;
 
-mod ball_joint;
-mod fixed_joint;
-// mod generic_joint;
-mod joint;
-mod joint_set;
-mod prismatic_joint;
 #[cfg(feature = "dim3")]
+pub use self::spherical_joint::SphericalJoint;
+
+mod fixed_joint;
+mod impulse_joint;
+mod joint_data;
+mod motor_model;
+mod multibody_joint;
+mod prismatic_joint;
 mod revolute_joint;
-mod spring_model;
+
+#[cfg(feature = "dim3")]
+mod spherical_joint;
diff --git a/src/dynamics/joint/spring_model.rs b/src/dynamics/joint/motor_model.rs
similarity index 66%
rename from src/dynamics/joint/spring_model.rs
rename to src/dynamics/joint/motor_model.rs
index c2c9ebd..e5a6549 100644
--- a/src/dynamics/joint/spring_model.rs
+++ b/src/dynamics/joint/motor_model.rs
@@ -3,9 +3,7 @@ use crate::math::Real;
 /// The spring-like model used for constraints resolution.
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-pub enum SpringModel {
-    /// No equation is solved.
-    Disabled,
+pub enum MotorModel {
     /// The solved spring-like equation is:
     /// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
     ///
@@ -16,21 +14,21 @@ pub enum SpringModel {
     /// The solved spring-like equation is:
     /// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
     AccelerationBased,
-    /// The solved spring-like equation is:
-    /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))`
-    ForceBased,
+    // /// The solved spring-like equation is:
+    // /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))`
+    // ForceBased,
 }
 
-impl Default for SpringModel {
+impl Default for MotorModel {
     fn default() -> Self {
-        SpringModel::VelocityBased
+        MotorModel::VelocityBased
     }
 }
 
-impl SpringModel {
+impl MotorModel {
     /// Combines the coefficients used for solving the spring equation.
     ///
-    /// Returns the new coefficients (stiffness, damping, inv_lhs_scale, keep_inv_lhs)
+    /// Returns the new coefficients (stiffness, damping, gamma, keep_inv_lhs)
     /// coefficients for the equivalent impulse-based equation. These new
     /// coefficients must be used in the following way:
     /// - `rhs = (stiffness * pos_err + damping * vel_err) / gamma`.
@@ -43,8 +41,8 @@ impl SpringModel {
         damping: Real,
     ) -> (Real, Real, Real, bool) {
         match self {
-            SpringModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
-            SpringModel::AccelerationBased => {
+            MotorModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
+            MotorModel::AccelerationBased => {
                 let effective_stiffness = stiffness * dt;
                 let effective_damping = damping * dt;
                 // TODO: Using gamma behaves very badly for some reasons.
@@ -52,14 +50,12 @@ impl SpringModel {
                 // and get back to this later.
                 // let gamma = effective_stiffness * dt + effective_damping;
                 (effective_stiffness, effective_damping, 1.0, true)
-            }
-            SpringModel::ForceBased => {
-                let effective_stiffness = stiffness * dt;
-                let effective_damping = damping * dt;
-                let gamma = effective_stiffness * dt + effective_damping;
-                (effective_stiffness, effective_damping, gamma, false)
-            }
-            SpringModel::Disabled => return (0.0, 0.0, 0.0, false),
+            } // MotorModel::ForceBased => {
+              //     let effective_stiffness = stiffness * dt;
+              //     let effective_damping = damping * dt;
+              //     let gamma = effective_stiffness * dt + effective_damping;
+              //     (effective_stiffness, effective_damping, gamma, false)
+              // }
         }
     }
 }
diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs
new file mode 100644
index 0000000..a701350
--- /dev/null
+++ b/src/dynamics/joint/multibody_joint/mod.rs
@@ -0,0 +1,15 @@
+//! MultibodyJoints using the reduced-coordinates formalism or using constraints.
+
+pub use self::multibody::Multibody;
+pub use self::multibody_joint::MultibodyJoint;
+pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet};
+pub use self::multibody_link::MultibodyLink;
+pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint};
+
+mod multibody;
+mod multibody_joint_set;
+mod multibody_link;
+mod multibody_workspace;
+
+mod multibody_joint;
+mod unit_multibody_joint;
diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs
new file mode 100644
index 0000000..70043d1
--- /dev/null
+++ b/src/dynamics/joint/multibody_joint/multibody.rs
@@ -0,0 +1,1021 @@
+use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
+use super::multibody_workspace::MultibodyWorkspace;
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
+use crate::dynamics::solver::AnyJointVelocityConstraint;
+use crate::dynamics::{
+    IntegrationParameters, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition,
+    RigidBodyType, RigidBodyVelocity,
+};
+#[cfg(feature = "dim3")]
+use crate::math::Matrix;
+use crate::math::{
+    AngDim, AngVector, Dim, Isometry, Jacobian, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
+};
+use crate::prelude::MultibodyJoint;
+use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix};
+use na::{
+    self, DMatrix, DVector, DVectorSlice, DVectorSliceMut, Dynamic, OMatrix, SMatrix, SVector, LU,
+};
+
+#[repr(C)]
+#[derive(Copy, Clone, Debug)]
+struct Force {
+    linear: Vector<Real>,
+    angular: AngVector<Real>,
+}
+
+impl Force {
+    fn new(linear: Vector<Real>, angular: AngVector<Real>) -> Self {
+        Self { linear, angular }
+    }
+
+    fn as_vector(&self) -> &SVector<Real, SPATIAL_DIM> {
+        unsafe { std::mem::transmute(self) }
+    }
+}
+
+#[cfg(feature = "dim2")]
+fn concat_rb_mass_matrix(mass: Real, inertia: Real) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
+    let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+    result[(0, 0)] = mass;
+    result[(1, 1)] = mass;
+    result[(2, 2)] = inertia;
+    result
+}
+
+#[cfg(feature = "dim3")]
+fn concat_rb_mass_matrix(
+    mass: Real,
+    inertia: Matrix<Real>,
+) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
+    let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+    result[(0, 0)] = mass;
+    result[(1, 1)] = mass;
+    result[(2, 2)] = mass;
+    result
+        .fixed_slice_mut::<ANG_DIM, ANG_DIM>(DIM, DIM)
+        .copy_from(&inertia);
+    result
+}
+
+/// An articulated body simulated using the reduced-coordinates approach.
+pub struct Multibody {
+    links: MultibodyLinkVec,
+    pub(crate) velocities: DVector<Real>,
+    pub(crate) damping: DVector<Real>,
+    pub(crate) accelerations: DVector<Real>,
+
+    body_jacobians: Vec<Jacobian<Real>>,
+    // FIXME: use sparse matrices.
+    augmented_mass: DMatrix<Real>,
+    inv_augmented_mass: LU<Real, Dynamic, Dynamic>,
+
+    acc_augmented_mass: DMatrix<Real>,
+    acc_inv_augmented_mass: LU<Real, Dynamic, Dynamic>,
+
+    ndofs: usize,
+    pub(crate) root_is_dynamic: bool,
+    pub(crate) solver_id: usize,
+
+    /*
+     * Workspaces.
+     */
+    workspace: MultibodyWorkspace,
+    coriolis_v: Vec<OMatrix<Real, Dim, Dynamic>>,
+    coriolis_w: Vec<OMatrix<Real, AngDim, Dynamic>>,
+    i_coriolis_dt: Jacobian<Real>,
+}
+
+impl Multibody {
+    /// Creates a new multibody with no link.
+    pub fn new() -> Self {
+        Multibody {
+            links: MultibodyLinkVec(Vec::new()),
+            velocities: DVector::zeros(0),
+            damping: DVector::zeros(0),
+            accelerations: DVector::zeros(0),
+            body_jacobians: Vec::new(),
+            augmented_mass: DMatrix::zeros(0, 0),
+            inv_augmented_mass: LU::new(DMatrix::zeros(0, 0)),
+            acc_augmented_mass: DMatrix::zeros(0, 0),
+            acc_inv_augmented_mass: LU::new(DMatrix::zeros(0, 0)),
+            ndofs: 0,
+            solver_id: 0,
+            workspace: MultibodyWorkspace::new(),
+            coriolis_v: Vec::new(),
+            coriolis_w: Vec::new(),
+            i_coriolis_dt: Jacobian::zeros(0),
+            root_is_dynamic: false,
+            // solver_workspace: Some(SolverWorkspace::new()),
+        }
+    }
+
+    pub fn with_root(handle: RigidBodyHandle) -> Self {
+        let mut mb = Multibody::new();
+        mb.root_is_dynamic = true;
+        let joint = MultibodyJoint::free(Isometry::identity());
+        mb.add_link(None, joint, handle);
+        mb
+    }
+
+    pub fn remove_link(self, to_remove: usize, joint_only: bool) -> Vec<Multibody> {
+        let mut result = vec![];
+        let mut link2mb = vec![usize::MAX; self.links.len()];
+        let mut link_id2new_id = vec![usize::MAX; self.links.len()];
+
+        for (i, mut link) in self.links.0.into_iter().enumerate() {
+            let is_new_root = (!joint_only && (i == 0 || link.parent_internal_id == to_remove))
+                || (joint_only && (i == 0 || i == to_remove));
+
+            if !joint_only && i == to_remove {
+                continue;
+            } else if is_new_root {
+                link2mb[i] = result.len();
+                result.push(Multibody::new());
+            } else {
+                link2mb[i] = link2mb[link.parent_internal_id]
+            }
+
+            let curr_mb = &mut result[link2mb[i]];
+            link_id2new_id[i] = curr_mb.links.len();
+
+            if is_new_root {
+                let joint = MultibodyJoint::fixed(*link.local_to_world());
+                link.state.joint = joint;
+            }
+
+            curr_mb.ndofs += link.joint().ndofs();
+            curr_mb.links.push(link);
+        }
+
+        // Adjust all the internal ids, and copy the data from the
+        // previous multibody to the new one.
+        for mb in &mut result {
+            mb.grow_buffers(mb.ndofs, mb.links.len());
+            mb.workspace.resize(mb.links.len(), mb.ndofs);
+
+            let mut assembly_id = 0;
+            for (i, link) in mb.links.iter_mut().enumerate() {
+                let link_ndofs = link.joint().ndofs();
+                mb.velocities
+                    .rows_mut(assembly_id, link_ndofs)
+                    .copy_from(&self.velocities.rows(link.assembly_id, link_ndofs));
+                mb.damping
+                    .rows_mut(assembly_id, link_ndofs)
+                    .copy_from(&self.damping.rows(link.assembly_id, link_ndofs));
+                mb.accelerations
+                    .rows_mut(assembly_id, link_ndofs)
+                    .copy_from(&self.accelerations.rows(link.assembly_id, link_ndofs));
+
+                link.internal_id = i;
+                link.assembly_id = assembly_id;
+                link.parent_internal_id = link_id2new_id[link.parent_internal_id];
+                assembly_id += link_ndofs;
+            }
+        }
+
+        result
+    }
+
+    pub fn append(&mut self, mut rhs: Multibody, parent: usize, joint: MultibodyJoint) {
+        let rhs_root_ndofs = rhs.links[0].state.joint.ndofs();
+        let rhs_copy_shift = self.ndofs + rhs_root_ndofs;
+        let rhs_copy_ndofs = rhs.ndofs - rhs_root_ndofs;
+
+        // Adjust the ids of all the rhs links except the first one.
+        let base_assembly_id = self.velocities.len() - rhs_root_ndofs + joint.ndofs();
+        let base_internal_id = self.links.len() + 1;
+        let base_parent_id = self.links.len();
+
+        for link in &mut rhs.links.0[1..] {
+            link.assembly_id += base_assembly_id;
+            link.internal_id += base_internal_id;
+            link.parent_internal_id += base_parent_id;
+        }
+
+        // Adjust the first link.
+        {
+            rhs.links[0].state.joint = joint;
+            rhs.links[0].assembly_id = self.velocities.len();
+            rhs.links[0].internal_id = self.links.len();
+            rhs.links[0].parent_internal_id = parent;
+        }
+
+        // Grow buffers and append data from rhs.
+        self.grow_buffers(
+            rhs_copy_ndofs + rhs.links[0].state.joint.ndofs(),
+            rhs.links.len(),
+        );
+
+        if rhs_copy_ndofs > 0 {
+            self.velocities
+                .rows_mut(rhs_copy_shift, rhs_copy_ndofs)
+                .copy_from(&rhs.velocities.rows(rhs_root_ndofs, rhs_copy_ndofs));
+            self.damping
+                .rows_mut(rhs_copy_shift, rhs_copy_ndofs)
+                .copy_from(&rhs.damping.rows(rhs_root_ndofs, rhs_copy_ndofs));
+            self.accelerations
+                .rows_mut(rhs_copy_shift, rhs_copy_ndofs)
+                .copy_from(&rhs.accelerations.rows(rhs_root_ndofs, rhs_copy_ndofs));
+        }
+
+        rhs.links[0]
+            .state
+            .joint
+            .default_damping(&mut self.damping.rows_mut(base_assembly_id, rhs_root_ndofs));
+
+        self.links.append(&mut rhs.links);
+        self.ndofs = self.velocities.len();
+        self.workspace.resize(self.links.len(), self.ndofs);
+    }
+
+    pub fn inv_augmented_mass(&self) -> &LU<Real, Dynamic, Dynamic> {
+        &self.inv_augmented_mass
+    }
+
+    /// The first link of this multibody.
+    #[inline]
+    pub fn root(&self) -> &MultibodyLink {
+        &self.links[0]
+    }
+
+    /// Mutable reference to the first link of this multibody.
+    #[inline]
+    pub fn root_mut(&mut self) -> &mut MultibodyLink {
+        &mut self.links[0]
+    }
+
+    /// Reference `i`-th multibody link of this multibody.
+    ///
+    /// Return `None` if there is less than `i + 1` multibody links.
+    #[inline]
+    pub fn link(&self, id: usize) -> Option<&MultibodyLink> {
+        self.links.get(id)
+    }
+
+    /// Mutable reference to the multibody link with the given id.
+    ///
+    /// Return `None` if the given id does not identifies a multibody link part of `self`.
+    #[inline]
+    pub fn link_mut(&mut self, id: usize) -> Option<&mut MultibodyLink> {
+        self.links.get_mut(id)
+    }
+
+    /// The links of this multibody with the given `name`.
+    pub fn links_with_name<'a>(
+        &'a self,
+        name: &'a str,
+    ) -> impl Iterator<Item = (usize, &'a MultibodyLink)> {
+        self.links
+            .iter()
+            .enumerate()
+            .filter(move |(_i, l)| l.name == name)
+    }
+
+    /// The number of links on this multibody.
+    pub fn num_links(&self) -> usize {
+        self.links.len()
+    }
+
+    /// Iterator through all the links of this multibody.
+    ///
+    /// All link are guaranteed to be yielded before its descendant.
+    pub fn links(&self) -> impl Iterator<Item = &MultibodyLink> {
+        self.links.iter()
+    }
+
+    /// Mutable iterator through all the links of this multibody.
+    ///
+    /// All link are guaranteed to be yielded before its descendant.
+    pub fn links_mut(&mut self) -> impl Iterator<Item = &mut MultibodyLink> {
+        self.links.iter_mut()
+    }
+
+    /// The vector of damping applied to this multibody.
+    #[inline]
+    pub fn damping(&self) -> &DVector<Real> {
+        &self.damping
+    }
+
+    /// Mutable vector of damping applied to this multibody.
+    #[inline]
+    pub fn damping_mut(&mut self) -> &mut DVector<Real> {
+        &mut self.damping
+    }
+
+    pub fn add_link(
+        &mut self,
+        parent: Option<usize>, // FIXME: should be a RigidBodyHandle?
+        mut dof: MultibodyJoint,
+        body: RigidBodyHandle,
+    ) -> &mut MultibodyLink {
+        assert!(
+            parent.is_none() || !self.links.is_empty(),
+            "Multibody::build_body: invalid parent id."
+        );
+
+        /*
+         * Compute the indices.
+         */
+        let assembly_id = self.velocities.len();
+        let internal_id = self.links.len();
+
+        /*
+         * Grow the buffers.
+         */
+        let ndofs = dof.ndofs();
+        self.grow_buffers(ndofs, 1);
+        self.ndofs += ndofs;
+
+        /*
+         * Setup default damping.
+         */
+        dof.default_damping(&mut self.damping.rows_mut(assembly_id, ndofs));
+
+        /*
+         * Create the multibody.
+         */
+        dof.update_jacobians(&self.velocities.as_slice()[assembly_id..]);
+        let local_to_parent = dof.body_to_parent();
+        let local_to_world;
+        let parent_to_world;
+
+        let parent_internal_id;
+        if let Some(parent) = parent {
+            parent_internal_id = parent;
+            let parent_link = &mut self.links[parent_internal_id];
+            parent_link.is_leaf = false;
+            parent_to_world = parent_link.state.local_to_world;
+            local_to_world = parent_link.state.local_to_world * local_to_parent;
+        } else {
+            parent_internal_id = 0;
+            parent_to_world = Isometry::identity();
+            local_to_world = local_to_parent;
+        }
+
+        let rb = MultibodyLink::new(
+            body,
+            internal_id,
+            assembly_id,
+            parent_internal_id,
+            dof,
+            parent_to_world,
+            local_to_world,
+            local_to_parent,
+        );
+
+        self.links.push(rb);
+        self.workspace.resize(self.links.len(), self.ndofs);
+
+        &mut self.links[internal_id]
+    }
+
+    fn grow_buffers(&mut self, ndofs: usize, num_jacobians: usize) {
+        let len = self.velocities.len();
+        self.velocities.resize_vertically_mut(len + ndofs, 0.0);
+        self.damping.resize_vertically_mut(len + ndofs, 0.0);
+        self.accelerations.resize_vertically_mut(len + ndofs, 0.0);
+        self.body_jacobians
+            .extend((0..num_jacobians).map(|_| Jacobian::zeros(0)));
+    }
+
+    pub fn update_acceleration<Bodies>(&mut self, bodies: &Bodies)
+    where
+        Bodies: ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyForces>
+            + ComponentSet<RigidBodyVelocity>,
+    {
+        if self.ndofs == 0 {
+            return; // Nothing to do.
+        }
+
+        self.accelerations.fill(0.0);
+
+        for i in 0..self.links.len() {
+            let link = &self.links[i];
+
+            let (rb_vels, rb_mprops, rb_forces): (
+                &RigidBodyVelocity,
+                &RigidBodyMassProps,
+                &RigidBodyForces,
+            ) = bodies.index_bundle(link.rigid_body.0);
+
+            let mut acc = link.velocity_dot_wrt_joint;
+
+            if i != 0 {
+                let parent_id = link.parent_internal_id;
+                let parent_link = &self.links[parent_id];
+
+                let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
+                    bodies.index_bundle(parent_link.rigid_body.0);
+
+                acc += self.workspace.accs[parent_id];
+                acc.linvel += parent_rb_vels.angvel.gcross(link.velocity_wrt_joint.linvel);
+                #[cfg(feature = "dim3")]
+                {
+                    acc.angvel += parent_rb_vels.angvel.cross(&link.velocity_wrt_joint.angvel);
+                }
+
+                let shift = rb_mprops.world_com - parent_rb_mprops.world_com;
+                let dvel = rb_vels.linvel - parent_rb_vels.linvel;
+
+                acc.linvel += parent_rb_vels.angvel.gcross(dvel);
+                acc.linvel += self.workspace.accs[parent_id].angvel.gcross(shift);
+            }
+
+            self.workspace.accs[i] = acc;
+
+            // TODO: should gyroscopic forces already be computed by the rigid-body itself
+            //       (at the same time that we add the gravity force)?
+            let gyroscopic;
+            let rb_inertia = rb_mprops.effective_angular_inertia();
+            let rb_mass = rb_mprops.effective_mass();
+
+            #[cfg(feature = "dim3")]
+            {
+                gyroscopic = rb_vels.angvel.cross(&(rb_inertia * rb_vels.angvel));
+            }
+            #[cfg(feature = "dim2")]
+            {
+                gyroscopic = 0.0;
+            }
+
+            let external_forces = Force::new(
+                rb_forces.force - rb_mass * acc.linvel,
+                rb_forces.torque - gyroscopic - rb_inertia * acc.angvel,
+            );
+            self.accelerations.gemv_tr(
+                1.0,
+                &self.body_jacobians[i],
+                external_forces.as_vector(),
+                1.0,
+            );
+        }
+
+        self.accelerations
+            .cmpy(-1.0, &self.damping, &self.velocities, 1.0);
+
+        self.acc_inv_augmented_mass
+            .solve_mut(&mut self.accelerations);
+    }
+
+    /// Computes the constant terms of the dynamics.
+    pub fn update_dynamics<Bodies>(&mut self, dt: Real, bodies: &mut Bodies)
+    where
+        Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyMassProps>,
+    {
+        /*
+         * Compute velocities.
+         * NOTE: this is needed for kinematic bodies too.
+         */
+        let link = &mut self.links[0];
+        let velocity_wrt_joint = link
+            .state
+            .joint
+            .jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
+        let velocity_dot_wrt_joint = link
+            .state
+            .joint
+            .jacobian_dot_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
+
+        link.velocity_dot_wrt_joint = velocity_dot_wrt_joint;
+        link.velocity_wrt_joint = velocity_wrt_joint;
+        bodies.set_internal(link.rigid_body.0, link.velocity_wrt_joint);
+
+        for i in 1..self.links.len() {
+            let (link, parent_link) = self.links.get_mut_with_parent(i);
+            let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0);
+            let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
+                bodies.index_bundle(parent_link.rigid_body.0);
+
+            let velocity_wrt_joint = link
+                .state
+                .joint
+                .jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
+            let velocity_dot_wrt_joint = link
+                .state
+                .joint
+                .jacobian_dot_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
+
+            link.velocity_dot_wrt_joint =
+                velocity_dot_wrt_joint.transformed(&parent_link.state.local_to_world);
+            link.velocity_wrt_joint =
+                velocity_wrt_joint.transformed(&parent_link.state.local_to_world);
+            let mut new_rb_vels = *parent_rb_vels + link.velocity_wrt_joint;
+            let shift = rb_mprops.world_com - parent_rb_mprops.world_com;
+            new_rb_vels.linvel += parent_rb_vels.angvel.gcross(shift);
+
+            bodies.set_internal(link.rigid_body.0, new_rb_vels);
+        }
+
+        /*
+         * Update augmented mass matrix.
+         */
+        self.update_inertias(dt, bodies);
+    }
+
+    fn update_body_next_jacobians<Bodies>(&mut self, bodies: &Bodies)
+    where
+        Bodies: ComponentSet<RigidBodyMassProps>,
+    {
+        for i in 0..self.links.len() {
+            let link = &self.links[i];
+            let rb_mprops = bodies.index(link.rigid_body.0);
+
+            if self.body_jacobians[i].ncols() != self.ndofs {
+                // FIXME: use a resize instead.
+                self.body_jacobians[i] = Jacobian::zeros(self.ndofs);
+            }
+
+            if i != 0 {
+                let parent_id = link.parent_internal_id;
+                let parent_link = &self.links[parent_id];
+                let parent_rb_mprops = bodies.index(parent_link.rigid_body.0);
+
+                let (link_j, parent_j) = self.body_jacobians.index_mut_const(i, parent_id);
+                link_j.copy_from(&parent_j);
+
+                {
+                    let mut link_j_v = link_j.fixed_rows_mut::<DIM>(0);
+                    let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM);
+
+                    let shift_tr = (link.state.local_to_world * rb_mprops.local_mprops.local_com
+                        - parent_link.state.local_to_world
+                            * parent_rb_mprops.local_mprops.local_com)
+                        .gcross_matrix_tr();
+                    link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0);
+                }
+            } else {
+                self.body_jacobians[i].fill(0.0);
+            }
+
+            let ndofs = link.state.joint.ndofs();
+            let mut tmp = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+            let mut link_joint_j = tmp.columns_mut(0, ndofs);
+            let mut link_j_part = self.body_jacobians[i].columns_mut(link.assembly_id, ndofs);
+            link.state
+                .joint
+                .jacobian(&link.state.parent_to_world, &mut link_joint_j);
+
+            link_j_part += link_joint_j;
+        }
+    }
+
+    fn update_inertias<Bodies>(&mut self, dt: Real, bodies: &Bodies)
+    where
+        Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyVelocity>,
+    {
+        if self.ndofs == 0 {
+            return; // Nothing to do.
+        }
+
+        if self.augmented_mass.ncols() != self.ndofs {
+            // TODO: do a resize instead of a full reallocation.
+            self.augmented_mass = DMatrix::zeros(self.ndofs, self.ndofs);
+            self.acc_augmented_mass = DMatrix::zeros(self.ndofs, self.ndofs);
+        } else {
+            self.augmented_mass.fill(0.0);
+            self.acc_augmented_mass.fill(0.0);
+        }
+
+        if self.coriolis_v.len() != self.links.len() {
+            self.coriolis_v.resize(
+                self.links.len(),
+                OMatrix::<Real, Dim, Dynamic>::zeros(self.ndofs),
+            );
+            self.coriolis_w.resize(
+                self.links.len(),
+                OMatrix::<Real, AngDim, Dynamic>::zeros(self.ndofs),
+            );
+            self.i_coriolis_dt = Jacobian::zeros(self.ndofs);
+        }
+
+        for i in 0..self.links.len() {
+            let link = &self.links[i];
+            let (rb_vels, rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
+                bodies.index_bundle(link.rigid_body.0);
+            let rb_mass = rb_mprops.effective_mass();
+            let rb_inertia = rb_mprops.effective_angular_inertia().into_matrix();
+
+            let body_jacobian = &self.body_jacobians[i];
+
+            #[allow(unused_mut)] // mut is needed for 3D but not for 2D.
+            let mut augmented_inertia = rb_inertia;
+
+            #[cfg(feature = "dim3")]
+            {
+                // Derivative of gyroscopic forces.
+                let gyroscopic_matrix = rb_vels.angvel.gcross_matrix() * rb_inertia
+                    - (rb_inertia * rb_vels.angvel).gcross_matrix();
+
+                augmented_inertia += gyroscopic_matrix * dt;
+            }
+
+            // TODO: optimize that (knowing the structure of the augmented inertia matrix).
+            // TODO: this could be better optimized in 2D.
+            let rb_mass_matrix_wo_gyro = concat_rb_mass_matrix(rb_mass, rb_inertia);
+            let rb_mass_matrix = concat_rb_mass_matrix(rb_mass, augmented_inertia);
+            self.augmented_mass
+                .quadform(1.0, &rb_mass_matrix_wo_gyro, body_jacobian, 1.0);
+            self.acc_augmented_mass
+                .quadform(1.0, &rb_mass_matrix, body_jacobian, 1.0);
+
+            /*
+             *
+             * Coriolis matrix.
+             *
+             */
+            let rb_j = &self.body_jacobians[i];
+            let rb_j_v = rb_j.fixed_rows::<DIM>(0);
+
+            let ndofs = link.state.joint.ndofs();
+
+            if i != 0 {
+                let parent_id = link.parent_internal_id;
+                let parent_link = &self.links[parent_id];
+                let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
+                    bodies.index_bundle(parent_link.rigid_body.0);
+                let parent_j = &self.body_jacobians[parent_id];
+                let parent_j_v = parent_j.fixed_rows::<DIM>(0);
+                let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM);
+                let parent_w = parent_rb_vels.angvel.gcross_matrix();
+
+                let (coriolis_v, parent_coriolis_v) = self.coriolis_v.index_mut2(i, parent_id);
+                let (coriolis_w, parent_coriolis_w) = self.coriolis_w.index_mut2(i, parent_id);
+
+                // JDot + JDot/u * qdot
+                coriolis_v.copy_from(&parent_coriolis_v);
+                coriolis_w.copy_from(&parent_coriolis_w);
+
+                let shift_cross =
+                    (rb_mprops.world_com - parent_rb_mprops.world_com).gcross_matrix_tr();
+                coriolis_v.gemm(1.0, &shift_cross, &parent_coriolis_w, 1.0);
+
+                // JDot
+                let dvel_cross = (rb_vels.linvel - parent_rb_vels.linvel).gcross_matrix_tr();
+                coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0);
+
+                // JDot/u * qdot
+                coriolis_v.gemm(
+                    1.0,
+                    &link.velocity_wrt_joint.linvel.gcross_matrix_tr(),
+                    &parent_j_w,
+                    1.0,
+                );
+                coriolis_v.gemm(1.0, &parent_w, &rb_j_v, 1.0);
+                coriolis_v.gemm(-1.0, &parent_w, &parent_j_v, 1.0);
+
+                #[cfg(feature = "dim3")]
+                {
+                    let vel_wrt_joint_w = link.velocity_wrt_joint.angvel.gcross_matrix();
+                    coriolis_w.gemm(-1.0, &vel_wrt_joint_w, &parent_j_w, 1.0);
+                }
+
+                // JDot
+                {
+                    let mut coriolis_v_part = coriolis_v.columns_mut(link.assembly_id, ndofs);
+
+                    let mut tmp1 = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+                    let mut rb_joint_j = tmp1.columns_mut(0, ndofs);
+                    link.state
+                        .joint
+                        .jacobian(&parent_link.state.local_to_world, &mut rb_joint_j);
+
+                    let rb_joint_j_v = rb_joint_j.fixed_rows::<DIM>(0);
+
+                    coriolis_v_part.gemm(1.0, &parent_w, &rb_joint_j_v, 1.0);
+
+                    #[cfg(feature = "dim3")]
+                    {
+                        let rb_joint_j_w = rb_joint_j.fixed_rows::<ANG_DIM>(DIM);
+                        let mut coriolis_w_part = coriolis_w.columns_mut(link.assembly_id, ndofs);
+                        coriolis_w_part.gemm(1.0, &parent_w, &rb_joint_j_w, 1.0);
+                    }
+                }
+            } else {
+                self.coriolis_v[i].fill(0.0);
+                self.coriolis_w[i].fill(0.0);
+            }
+
+            let coriolis_v = &mut self.coriolis_v[i];
+            let coriolis_w = &mut self.coriolis_w[i];
+
+            {
+                let mut tmp1 = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+                let mut tmp2 = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+                let mut rb_joint_j_dot = tmp1.columns_mut(0, ndofs);
+                let mut rb_joint_j_dot_veldiff = tmp2.columns_mut(0, ndofs);
+
+                link.state
+                    .joint
+                    .jacobian_dot(&link.state.parent_to_world, &mut rb_joint_j_dot);
+                link.state.joint.jacobian_dot_veldiff_mul_coordinates(
+                    &link.state.parent_to_world,
+                    &self.velocities.as_slice()[link.assembly_id..],
+                    &mut rb_joint_j_dot_veldiff,
+                );
+
+                let rb_joint_j_v_dot = rb_joint_j_dot.fixed_rows::<DIM>(0);
+                let rb_joint_j_w_dot = rb_joint_j_dot.fixed_rows::<ANG_DIM>(DIM);
+                let rb_joint_j_v_dot_veldiff = rb_joint_j_dot_veldiff.fixed_rows::<DIM>(0);
+                let rb_joint_j_w_dot_veldiff = rb_joint_j_dot_veldiff.fixed_rows::<ANG_DIM>(DIM);
+
+                let mut coriolis_v_part = coriolis_v.columns_mut(link.assembly_id, ndofs);
+                let mut coriolis_w_part = coriolis_w.columns_mut(link.assembly_id, ndofs);
+
+                // JDot
+                coriolis_v_part += rb_joint_j_v_dot;
+                coriolis_w_part += rb_joint_j_w_dot;
+
+                // JDot/u * qdot
+                coriolis_v_part += rb_joint_j_v_dot_veldiff;
+                coriolis_w_part += rb_joint_j_w_dot_veldiff;
+            }
+
+            /*
+             * Meld with the mass matrix.
+             */
+            {
+                let mut i_coriolis_dt_v = self.i_coriolis_dt.fixed_rows_mut::<DIM>(0);
+                i_coriolis_dt_v.copy_from(coriolis_v);
+                i_coriolis_dt_v *= rb_mass * dt;
+            }
+
+            #[cfg(feature = "dim2")]
+            {
+                let mut i_coriolis_dt_w = self.i_coriolis_dt.fixed_rows_mut::<ANG_DIM>(DIM);
+                // NOTE: this is just an axpy, but on row columns.
+                i_coriolis_dt_w.zip_apply(&coriolis_w, |o, x| *o = x * dt * rb_inertia);
+            }
+            #[cfg(feature = "dim3")]
+            {
+                let mut i_coriolis_dt_w = self.i_coriolis_dt.fixed_rows_mut::<ANG_DIM>(DIM);
+                i_coriolis_dt_w.gemm(dt, &rb_inertia, &coriolis_w, 0.0);
+            }
+
+            self.acc_augmented_mass
+                .gemm_tr(1.0, &rb_j, &self.i_coriolis_dt, 1.0);
+        }
+
+        /*
+         * Damping.
+         */
+        for i in 0..self.ndofs {
+            self.acc_augmented_mass[(i, i)] += self.damping[i] * dt;
+            self.augmented_mass[(i, i)] += self.damping[i] * dt;
+        }
+
+        // FIXME: avoid allocation inside LU at each timestep.
+        self.acc_inv_augmented_mass = LU::new(self.acc_augmented_mass.clone());
+        self.inv_augmented_mass = LU::new(self.augmented_mass.clone());
+        // self.inv_augmented_mass = self.acc_inv_augmented_mass.clone();
+    }
+
+    /// The generalized velocity at the multibody_joint of the given link.
+    #[inline]
+    pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorSlice<Real> {
+        let ndofs = link.joint().ndofs();
+        DVectorSlice::from_slice(
+            &self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs],
+            ndofs,
+        )
+    }
+
+    #[inline]
+    pub fn generalized_acceleration(&self) -> DVectorSlice<Real> {
+        self.accelerations.rows(0, self.ndofs)
+    }
+
+    #[inline]
+    pub fn generalized_velocity(&self) -> DVectorSlice<Real> {
+        self.velocities.rows(0, self.ndofs)
+    }
+
+    #[inline]
+    pub fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<Real> {
+        self.velocities.rows_mut(0, self.ndofs)
+    }
+
+    #[inline]
+    pub fn integrate_next(&mut self, dt: Real) {
+        for rb in self.links.iter_mut() {
+            rb.state
+                .joint
+                .integrate(dt, &self.velocities.as_slice()[rb.assembly_id..])
+        }
+    }
+
+    pub fn apply_next_displacements(&mut self, disp: &[Real]) {
+        for link in self.links.iter_mut() {
+            link.state
+                .joint
+                .apply_displacement(&disp[link.assembly_id..])
+        }
+    }
+
+    pub fn update_root_type<Bodies>(&mut self, bodies: &mut Bodies)
+    where
+        Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyPosition>,
+    {
+        let rb_type: Option<&RigidBodyType> = bodies.get(self.links[0].rigid_body.0);
+        if let Some(rb_type) = rb_type {
+            let rb_pos: &RigidBodyPosition = bodies.index(self.links[0].rigid_body.0);
+
+            if rb_type.is_dynamic() != self.root_is_dynamic {
+                if rb_type.is_dynamic() {
+                    let free_joint = MultibodyJoint::free(rb_pos.position);
+                    let prev_root_ndofs = self.links[0].joint().ndofs();
+                    self.links[0].state.joint = free_joint;
+                    self.links[0].assembly_id = 0;
+                    self.ndofs += SPATIAL_DIM;
+
+                    self.velocities = self.velocities.clone().insert_rows(0, SPATIAL_DIM, 0.0);
+                    self.damping = self.damping.clone().insert_rows(0, SPATIAL_DIM, 0.0);
+                    self.accelerations =
+                        self.accelerations.clone().insert_rows(0, SPATIAL_DIM, 0.0);
+
+                    for link in &mut self.links[1..] {
+                        link.assembly_id += SPATIAL_DIM - prev_root_ndofs;
+                    }
+                } else {
+                    assert!(self.velocities.len() >= SPATIAL_DIM);
+                    assert!(self.damping.len() >= SPATIAL_DIM);
+                    assert!(self.accelerations.len() >= SPATIAL_DIM);
+
+                    let fixed_joint = MultibodyJoint::fixed(rb_pos.position);
+                    let prev_root_ndofs = self.links[0].joint().ndofs();
+                    self.links[0].state.joint = fixed_joint;
+                    self.links[0].assembly_id = 0;
+                    self.ndofs -= prev_root_ndofs;
+
+                    if self.ndofs == 0 {
+                        self.velocities = DVector::zeros(0);
+                        self.damping = DVector::zeros(0);
+                        self.accelerations = DVector::zeros(0);
+                    } else {
+                        self.velocities =
+                            self.velocities.index((prev_root_ndofs.., 0)).into_owned();
+                        self.damping = self.damping.index((prev_root_ndofs.., 0)).into_owned();
+                        self.accelerations = self
+                            .accelerations
+                            .index((prev_root_ndofs.., 0))
+                            .into_owned();
+                    }
+
+                    for link in &mut self.links[1..] {
+                        link.assembly_id -= prev_root_ndofs;
+                    }
+                }
+
+                self.root_is_dynamic = rb_type.is_dynamic();
+            }
+
+            // Make sure the positions are properly set to match the rigid-body’s.
+            if self.links[0].state.joint.data.locked_axes.is_empty() {
+                self.links[0].state.joint.set_free_pos(rb_pos.position);
+            } else {
+                self.links[0].state.joint.data.local_frame1 = rb_pos.position;
+            }
+        }
+    }
+
+    pub fn forward_kinematics_next<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
+    where
+        Bodies: ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyPosition>,
+    {
+        // Special case for the root, which has no parent.
+        {
+            let link = &mut self.links[0];
+            link.state
+                .joint
+                .update_jacobians(&self.velocities.as_slice());
+            link.state.local_to_parent = link.state.joint.body_to_parent();
+            link.state.local_to_world = link.state.local_to_parent;
+
+            bodies.map_mut_internal(link.rigid_body.0, |poss: &mut RigidBodyPosition| {
+                poss.next_position = link.state.local_to_world;
+            });
+
+            if update_mass_props {
+                bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
+                    mprops.update_world_mass_properties(&link.state.local_to_world)
+                });
+            }
+        }
+
+        // Handle the children. They all have a parent within this multibody.
+        for i in 1..self.links.len() {
+            let (link, parent_link) = self.links.get_mut_with_parent(i);
+
+            link.state
+                .joint
+                .update_jacobians(&self.velocities.as_slice()[link.assembly_id..]);
+            link.state.local_to_parent = link.state.joint.body_to_parent();
+            link.state.local_to_world =
+                parent_link.state.local_to_world * link.state.local_to_parent;
+            link.state.parent_to_world = parent_link.state.local_to_world;
+
+            bodies.map_mut_internal(link.rigid_body.0, |poss: &mut RigidBodyPosition| {
+                poss.next_position = link.state.local_to_world;
+            });
+
+            let rb_type: &RigidBodyType = bodies.index(link.rigid_body.0);
+            assert_eq!(
+                *rb_type,
+                RigidBodyType::Dynamic,
+                "A rigid-body that is not at the root of a multibody must be dynamic."
+            );
+
+            if update_mass_props {
+                bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
+                    mprops.update_world_mass_properties(&link.state.local_to_world)
+                });
+            }
+        }
+
+        /*
+         * Compute body jacobians.
+         */
+        self.update_body_next_jacobians(bodies);
+    }
+
+    #[inline]
+    pub fn ndofs(&self) -> usize {
+        self.ndofs
+    }
+
+    pub fn fill_jacobians(
+        &self,
+        link_id: usize,
+        unit_force: Vector<Real>,
+        unit_torque: SVector<Real, ANG_DIM>,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+    ) -> (Real, Real) {
+        if self.ndofs == 0 {
+            return (0.0, 0.0);
+        }
+
+        let wj_id = *j_id + self.ndofs;
+        let force = Force {
+            linear: unit_force,
+            #[cfg(feature = "dim2")]
+            angular: unit_torque[0],
+            #[cfg(feature = "dim3")]
+            angular: unit_torque,
+        };
+
+        let link = &self.links[link_id];
+        let mut out_j = jacobians.rows_mut(*j_id, self.ndofs);
+        self.body_jacobians[link.internal_id].tr_mul_to(force.as_vector(), &mut out_j);
+
+        // TODO: Optimize with a copy_nonoverlapping?
+        for i in 0..self.ndofs {
+            jacobians[wj_id + i] = jacobians[*j_id + i];
+        }
+
+        {
+            let mut out_invm_j = jacobians.rows_mut(wj_id, self.ndofs);
+            self.inv_augmented_mass.solve_mut(&mut out_invm_j);
+        }
+
+        let j = jacobians.rows(*j_id, self.ndofs);
+        let invm_j = jacobians.rows(wj_id, self.ndofs);
+        *j_id += self.ndofs * 2;
+
+        (j.dot(&invm_j), j.dot(&self.generalized_velocity()))
+    }
+
+    #[inline]
+    pub fn has_active_internal_constraints(&self) -> bool {
+        self.links()
+            .any(|link| link.joint().num_velocity_constraints() != 0)
+    }
+
+    #[inline]
+    pub fn generate_internal_constraints(
+        &self,
+        params: &IntegrationParameters,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+        out: &mut Vec<AnyJointVelocityConstraint>,
+    ) {
+        let num_constraints: usize = self
+            .links
+            .iter()
+            .map(|l| l.joint().num_velocity_constraints())
+            .sum();
+
+        let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2;
+        if jacobians.nrows() < required_jacobian_len {
+            jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
+        }
+
+        for link in self.links.iter() {
+            link.joint()
+                .velocity_constraints(params, self, link, 0, j_id, jacobians, out);
+        }
+    }
+}
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs
new file mode 100644
index 0000000..1e8522e
--- /dev/null
+++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs
@@ -0,0 +1,571 @@
+use crate::dynamics::solver::AnyJointVelocityConstraint;
+use crate::dynamics::{
+    joint, FixedJoint, IntegrationParameters, JointAxesMask, JointData, Multibody, MultibodyLink,
+    RigidBodyVelocity,
+};
+use crate::math::{
+    Isometry, JacobianSliceMut, Matrix, Real, Rotation, SpacialVector, Translation, Vector,
+    ANG_DIM, DIM, SPATIAL_DIM,
+};
+use crate::utils::WCross;
+use na::{DVector, DVectorSliceMut};
+#[cfg(feature = "dim3")]
+use {
+    crate::utils::WCrossMatrix,
+    na::{UnitQuaternion, Vector3, VectorSlice3},
+};
+
+#[derive(Copy, Clone, Debug)]
+pub struct MultibodyJoint {
+    pub data: JointData,
+    pub(crate) coords: SpacialVector<Real>,
+    pub(crate) joint_rot: Rotation<Real>,
+    jacobian_v: Matrix<Real>,
+    jacobian_dot_v: Matrix<Real>,
+    jacobian_dot_veldiff_v: Matrix<Real>,
+}
+
+#[cfg(feature = "dim2")]
+fn revolute_locked_axes() -> JointAxesMask {
+    JointAxesMask::X | JointAxesMask::Y
+}
+
+#[cfg(feature = "dim3")]
+fn revolute_locked_axes() -> JointAxesMask {
+    JointAxesMask::X
+        | JointAxesMask::Y
+        | JointAxesMask::Z
+        | JointAxesMask::ANG_Y
+        | JointAxesMask::ANG_Z
+}
+
+impl MultibodyJoint {
+    pub fn new(data: JointData) -> Self {
+        Self {
+            data,
+            coords: na::zero(),
+            joint_rot: Rotation::identity(),
+            jacobian_v: na::zero(),
+            jacobian_dot_v: na::zero(),
+            jacobian_dot_veldiff_v: na::zero(),
+        }
+    }
+
+    pub(crate) fn free(pos: Isometry<Real>) -> Self {
+        let mut result = Self::new(JointData::default());
+        result.set_free_pos(pos);
+        result
+    }
+
+    pub(crate) fn fixed(pos: Isometry<Real>) -> Self {
+        Self::new(FixedJoint::new().local_frame1(pos).into())
+    }
+
+    pub(crate) fn set_free_pos(&mut self, pos: Isometry<Real>) {
+        self.coords
+            .fixed_rows_mut::<DIM>(0)
+            .copy_from(&pos.translation.vector);
+        self.joint_rot = pos.rotation;
+    }
+
+    pub fn local_joint_rot(&self) -> &Rotation<Real> {
+        &self.joint_rot
+    }
+
+    fn num_free_lin_dofs(&self) -> usize {
+        let locked_bits = self.data.locked_axes.bits();
+        DIM - (locked_bits & ((1 << DIM) - 1)).count_ones() as usize
+    }
+
+    /// The number of degrees of freedom allowed by the multibody_joint.
+    pub fn ndofs(&self) -> usize {
+        SPATIAL_DIM - self.data.locked_axes.bits().count_ones() as usize
+    }
+
+    /// The position of the multibody link containing this multibody_joint relative to its parent.
+    pub fn body_to_parent(&self) -> Isometry<Real> {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            self.data.local_frame1.translation
+                * self.joint_rot
+                * self.data.local_frame2.translation.inverse()
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let mut transform = self.joint_rot * self.data.local_frame2.inverse();
+
+            for i in 0..DIM {
+                if (locked_bits & (1 << i)) == 0 {
+                    transform = Translation::from(Vector::ith(i, self.coords[i])) * transform;
+                }
+            }
+
+            self.data.local_frame1 * transform
+        }
+    }
+
+    /// Integrate the position of this multibody_joint.
+    pub fn integrate(&mut self, dt: Real, vels: &[Real]) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            #[cfg(feature = "dim3")]
+            let axis = self.data.local_frame1 * Vector::x_axis();
+            self.coords[DIM] += vels[0] * dt;
+
+            #[cfg(feature = "dim2")]
+            {
+                self.joint_rot = Rotation::from_angle(self.coords[DIM]);
+            }
+            #[cfg(feature = "dim3")]
+            {
+                self.joint_rot = Rotation::from_axis_angle(&axis, self.coords[DIM]);
+            }
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let mut curr_free_dof = 0;
+
+            for i in 0..DIM {
+                if (locked_bits & (1 << i)) == 0 {
+                    self.coords[i] += vels[curr_free_dof] * dt;
+                    curr_free_dof += 1;
+                }
+            }
+
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]);
+                    let disp = UnitQuaternion::new_eps(angvel * dt, 0.0);
+                    self.joint_rot = disp * self.joint_rot;
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Apply a displacement to the multibody_joint.
+    pub fn apply_displacement(&mut self, disp: &[Real]) {
+        self.integrate(1.0, disp);
+    }
+
+    /// Update the jacobians of this multibody_joint.
+    pub fn update_jacobians(&mut self, vels: &[Real]) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            #[cfg(feature = "dim2")]
+            let axis = 1.0;
+            #[cfg(feature = "dim3")]
+            let axis = self.data.local_frame1 * Vector::x_axis();
+            let body_shift = self.data.local_frame2.translation.vector;
+            let shift = self.joint_rot * -body_shift;
+            let shift_dot_veldiff = axis.gcross(shift);
+
+            #[cfg(feature = "dim2")]
+            {
+                self.jacobian_v.column_mut(0).copy_from(&axis.gcross(shift));
+            }
+            #[cfg(feature = "dim3")]
+            {
+                self.jacobian_v.column_mut(0).copy_from(&axis.gcross(shift));
+            }
+            self.jacobian_dot_veldiff_v
+                .column_mut(0)
+                .copy_from(&axis.gcross(shift_dot_veldiff));
+            self.jacobian_dot_v
+                .column_mut(0)
+                .copy_from(&(axis.gcross(shift_dot_veldiff) * vels[0]));
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let num_free_lin_dofs = self.num_free_lin_dofs();
+                    let inv_frame2 = self.data.local_frame2.inverse();
+                    let shift = self.joint_rot * inv_frame2.translation.vector;
+                    let angvel =
+                        VectorSlice3::from_slice(&vels[num_free_lin_dofs..num_free_lin_dofs + 3]);
+                    let inv_rotmat2 = inv_frame2.rotation.to_rotation_matrix().into_inner();
+
+                    self.jacobian_v = inv_rotmat2 * shift.gcross_matrix().transpose();
+                    self.jacobian_dot_v =
+                        inv_rotmat2 * angvel.cross(&shift).gcross_matrix().transpose();
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Sets in `out` the non-zero entries of the multibody_joint jacobian transformed by `transform`.
+    pub fn jacobian(&self, transform: &Isometry<Real>, out: &mut JacobianSliceMut<Real>) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            #[cfg(feature = "dim2")]
+            let axis = 1.0;
+            #[cfg(feature = "dim3")]
+            let axis = self.data.local_frame1 * Vector::x();
+            let jacobian = RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis);
+            out.copy_from(jacobian.transformed(transform).as_vector())
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let mut curr_free_dof = 0;
+
+            for i in 0..DIM {
+                if (locked_bits & (1 << i)) == 0 {
+                    let transformed_axis = transform * self.data.local_frame1 * Vector::ith(i, 1.0);
+                    out.fixed_slice_mut::<DIM, 1>(0, curr_free_dof)
+                        .copy_from(&transformed_axis);
+                    curr_free_dof += 1;
+                }
+            }
+
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let rotmat = transform.rotation.to_rotation_matrix();
+                    out.fixed_slice_mut::<3, 3>(0, curr_free_dof)
+                        .copy_from(&(rotmat * self.jacobian_v));
+                    out.fixed_slice_mut::<3, 3>(3, curr_free_dof)
+                        .copy_from(rotmat.matrix());
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Sets in `out` the non-zero entries of the time-derivative of the multibody_joint jacobian transformed by `transform`.
+    pub fn jacobian_dot(&self, transform: &Isometry<Real>, out: &mut JacobianSliceMut<Real>) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            let jacobian = RigidBodyVelocity::from_vectors(
+                self.jacobian_dot_v.column(0).into_owned(),
+                na::zero(),
+            );
+            out.copy_from(jacobian.transformed(transform).as_vector())
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let num_free_lin_dofs = self.num_free_lin_dofs();
+                    let rotmat = transform.rotation.to_rotation_matrix();
+                    out.fixed_slice_mut::<3, 3>(0, num_free_lin_dofs)
+                        .copy_from(&(rotmat * self.jacobian_dot_v));
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Sets in `out` the non-zero entries of the velocity-derivative of the time-derivative of the multibody_joint jacobian transformed by `transform`.
+    pub fn jacobian_dot_veldiff_mul_coordinates(
+        &self,
+        transform: &Isometry<Real>,
+        acc: &[Real],
+        out: &mut JacobianSliceMut<Real>,
+    ) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            let jacobian = RigidBodyVelocity::from_vectors(
+                self.jacobian_dot_veldiff_v.column(0).into_owned(),
+                na::zero(),
+            );
+            out.copy_from((jacobian.transformed(transform) * acc[0]).as_vector())
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let num_free_lin_dofs = self.num_free_lin_dofs();
+                    let angvel =
+                        Vector3::from_row_slice(&acc[num_free_lin_dofs..num_free_lin_dofs + 3]);
+                    let rotmat = transform.rotation.to_rotation_matrix();
+                    let res = rotmat * angvel.gcross_matrix() * self.jacobian_v;
+                    out.fixed_slice_mut::<3, 3>(0, num_free_lin_dofs)
+                        .copy_from(&res);
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Multiply the multibody_joint jacobian by generalized velocities to obtain the
+    /// relative velocity of the multibody link containing this multibody_joint.
+    pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            #[cfg(feature = "dim2")]
+            let axis = 1.0;
+            #[cfg(feature = "dim3")]
+            let axis = self.data.local_frame1 * Vector::x();
+            RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis) * acc[0]
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let mut result = RigidBodyVelocity::zero();
+            let mut curr_free_dof = 0;
+
+            for i in 0..DIM {
+                if (locked_bits & (1 << i)) == 0 {
+                    result.linvel += self.data.local_frame1 * Vector::ith(i, acc[curr_free_dof]);
+                    curr_free_dof += 1;
+                }
+            }
+
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let angvel = Vector3::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]);
+                    let linvel = self.jacobian_v * angvel;
+                    result += RigidBodyVelocity::new(linvel, angvel);
+                }
+                _ => unreachable!(),
+            }
+            result
+        }
+    }
+
+    /// Multiply the multibody_joint jacobian by generalized accelerations to obtain the
+    /// relative acceleration of the multibody link containing this multibody_joint.
+    pub fn jacobian_dot_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            RigidBodyVelocity::from_vectors(self.jacobian_dot_v.column(0).into_owned(), na::zero())
+                * acc[0]
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => {
+                    /* No free dofs. */
+                    RigidBodyVelocity::zero()
+                }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let num_free_lin_dofs = self.num_free_lin_dofs();
+                    let angvel =
+                        Vector3::from_row_slice(&acc[num_free_lin_dofs..num_free_lin_dofs + 3]);
+                    let linvel = self.jacobian_dot_v * angvel;
+                    RigidBodyVelocity::new(linvel, na::zero())
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Fill `out` with the non-zero entries of a damping that can be applied by default to ensure a good stability of the multibody_joint.
+    pub fn default_damping(&self, out: &mut DVectorSliceMut<Real>) {
+        let locked_bits = self.data.locked_axes.bits();
+        let mut curr_free_dof = self.num_free_lin_dofs();
+
+        // A default damping only for the angular dofs
+        for i in DIM..SPATIAL_DIM {
+            if locked_bits & (1 << i) == 0 {
+                // This is a free angular DOF.
+                out[curr_free_dof] = 0.2;
+                curr_free_dof += 1;
+            }
+        }
+    }
+
+    /// Maximum number of velocity constrains that can be generated by this multibody_joint.
+    pub fn num_velocity_constraints(&self) -> usize {
+        let locked_bits = self.data.locked_axes.bits();
+        let limit_bits = self.data.limit_axes.bits();
+        let motor_bits = self.data.motor_axes.bits();
+        let mut num_constraints = 0;
+
+        for i in 0..SPATIAL_DIM {
+            if (locked_bits & (1 << i)) == 0 {
+                if (limit_bits & (1 << i)) != 0 {
+                    num_constraints += 1;
+                }
+                if (motor_bits & (1 << i)) != 0 {
+                    num_constraints += 1;
+                }
+            }
+        }
+
+        num_constraints
+    }
+
+    /// Initialize and generate velocity constraints to enforce, e.g., multibody_joint limits and motors.
+    pub fn velocity_constraints(
+        &self,
+        params: &IntegrationParameters,
+        multibody: &Multibody,
+        link: &MultibodyLink,
+        dof_id: usize,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+        constraints: &mut Vec<AnyJointVelocityConstraint>,
+    ) {
+        let locked_bits = self.data.locked_axes.bits();
+        let limit_bits = self.data.limit_axes.bits();
+        let motor_bits = self.data.motor_axes.bits();
+        let mut curr_free_dof = 0;
+
+        for i in 0..DIM {
+            if (locked_bits & (1 << i)) == 0 {
+                if (limit_bits & (1 << i)) != 0 {
+                    joint::unit_joint_limit_constraint(
+                        params,
+                        multibody,
+                        link,
+                        [self.data.limits[i].min, self.data.limits[i].max],
+                        self.coords[i],
+                        dof_id + curr_free_dof,
+                        j_id,
+                        jacobians,
+                        constraints,
+                    );
+                }
+
+                if (motor_bits & (1 << i)) != 0 {
+                    joint::unit_joint_motor_constraint(
+                        params,
+                        multibody,
+                        link,
+                        &self.data.motors[i],
+                        self.coords[i],
+                        dof_id + curr_free_dof,
+                        j_id,
+                        jacobians,
+                        constraints,
+                    );
+                }
+                curr_free_dof += 1;
+            }
+        }
+
+        /*
+        let locked_ang_bits = locked_bits >> DIM;
+        let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+        match num_free_ang_dofs {
+            0 => { /* No free dofs. */ }
+            1 => {}
+            2 => {
+                todo!()
+            }
+            3 => {}
+            _ => unreachable!(),
+        }
+         */
+        // TODO: we should make special cases for multi-angular-dofs limits/motors
+        for i in DIM..SPATIAL_DIM {
+            if (locked_bits & (1 << i)) == 0 {
+                if (limit_bits & (1 << i)) != 0 {
+                    joint::unit_joint_limit_constraint(
+                        params,
+                        multibody,
+                        link,
+                        [self.data.limits[i].min, self.data.limits[i].max],
+                        self.coords[i],
+                        dof_id + curr_free_dof,
+                        j_id,
+                        jacobians,
+                        constraints,
+                    );
+                }
+
+                if (motor_bits & (1 << i)) != 0 {
+                    joint::unit_joint_motor_constraint(
+                        params,
+                        multibody,
+                        link,
+                        &self.data.motors[i],
+                        self.coords[i],
+                        dof_id + curr_free_dof,
+                        j_id,
+                        jacobians,
+                        constraints,
+                    );
+                }
+                curr_free_dof += 1;
+            }
+        }
+    }
+}
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
new file mode 100644
index 0000000..8337158
--- /dev/null
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -0,0 +1,352 @@
+use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut};
+use crate::dynamics::joint::MultibodyLink;
+use crate::dynamics::{
+    IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
+    RigidBodyIds, RigidBodyType,
+};
+use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
+use crate::parry::partitioning::IndexedData;
+use std::ops::Index;
+
+/// The unique handle of an multibody_joint added to a `MultibodyJointSet`.
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[repr(transparent)]
+pub struct MultibodyJointHandle(pub crate::data::arena::Index);
+
+/// The temporary index of a multibody added to a `MultibodyJointSet`.
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[repr(transparent)]
+pub struct MultibodyIndex(pub crate::data::arena::Index);
+
+impl MultibodyJointHandle {
+    /// Converts this handle into its (index, generation) components.
+    pub fn into_raw_parts(self) -> (u32, u32) {
+        self.0.into_raw_parts()
+    }
+
+    /// Reconstructs an handle from its (index, generation) components.
+    pub fn from_raw_parts(id: u32, generation: u32) -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(id, generation))
+    }
+
+    /// An always-invalid rigid-body handle.
+    pub fn invalid() -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(
+            crate::INVALID_U32,
+            crate::INVALID_U32,
+        ))
+    }
+}
+
+impl Default for MultibodyJointHandle {
+    fn default() -> Self {
+        Self::invalid()
+    }
+}
+
+impl IndexedData for MultibodyJointHandle {
+    fn default() -> Self {
+        Self(IndexedData::default())
+    }
+    fn index(&self) -> usize {
+        self.0.index()
+    }
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+pub struct MultibodyJointLink {
+    pub graph_id: RigidBodyGraphIndex,
+    pub multibody: MultibodyIndex,
+    pub id: usize,
+}
+
+impl Default for MultibodyJointLink {
+    fn default() -> Self {
+        Self {
+            graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32),
+            multibody: MultibodyIndex(crate::data::arena::Index::from_raw_parts(
+                crate::INVALID_U32,
+                crate::INVALID_U32,
+            )),
+            id: 0,
+        }
+    }
+}
+
+/// A set of rigid bodies that can be handled by a physics pipeline.
+pub struct MultibodyJointSet {
+    pub(crate) multibodies: Arena<Multibody>, // NOTE: a Slab would be sufficient.
+    pub(crate) rb2mb: Coarena<MultibodyJointLink>,
+    // NOTE: this is mostly for the island extraction. So perhaps we won’t need
+    //       that any more in the future when we improve our island builder.
+    pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
+}
+
+impl MultibodyJointSet {
+    /// Create a new empty set of multibodies.
+    pub fn new() -> Self {
+        Self {
+            multibodies: Arena::new(),
+            rb2mb: Coarena::new(),
+            connectivity_graph: InteractionGraph::new(),
+        }
+    }
+
+    pub fn iter(&self) -> impl Iterator<Item = (MultibodyJointHandle, &Multibody, &MultibodyLink)> {
+        self.rb2mb
+            .iter()
+            .filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user.
+            .map(|(h, link)| {
+                let mb = &self.multibodies[link.multibody.0];
+                (MultibodyJointHandle(h), mb, mb.link(link.id).unwrap())
+            })
+    }
+
+    /// Inserts a new multibody_joint into this set.
+    pub fn insert(
+        &mut self,
+        body1: RigidBodyHandle,
+        body2: RigidBodyHandle,
+        data: impl Into<JointData>,
+    ) -> Option<MultibodyJointHandle> {
+        let data = data.into();
+        let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {
+            let mb_handle = self.multibodies.insert(Multibody::with_root(body1));
+            MultibodyJointLink {
+                graph_id: self.connectivity_graph.graph.add_node(body1),
+                multibody: MultibodyIndex(mb_handle),
+                id: 0,
+            }
+        });
+
+        let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| {
+            let mb_handle = self.multibodies.insert(Multibody::with_root(body2));
+            MultibodyJointLink {
+                graph_id: self.connectivity_graph.graph.add_node(body2),
+                multibody: MultibodyIndex(mb_handle),
+                id: 0,
+            }
+        });
+
+        if link1.multibody == link2.multibody || link2.id != 0 {
+            // This would introduce an invalid configuration.
+            return None;
+        }
+
+        self.connectivity_graph
+            .graph
+            .add_edge(link1.graph_id, link2.graph_id, ());
+        self.rb2mb.insert(body1.0, link1);
+        self.rb2mb.insert(body2.0, link2);
+
+        let mb2 = self.multibodies.remove(link2.multibody.0).unwrap();
+        let multibody1 = &mut self.multibodies[link1.multibody.0];
+
+        for mb_link2 in mb2.links() {
+            let link = self.rb2mb.get_mut(mb_link2.rigid_body.0).unwrap();
+            link.multibody = link1.multibody;
+            link.id += multibody1.num_links();
+        }
+
+        multibody1.append(mb2, link1.id, MultibodyJoint::new(data));
+
+        // Because each rigid-body can only have one parent link,
+        // we can use the second rigid-body’s handle as the multibody_joint’s
+        // handle.
+        Some(MultibodyJointHandle(body2.0))
+    }
+
+    /// Removes an multibody_joint from this set.
+    pub fn remove<Bodies>(
+        &mut self,
+        handle: MultibodyJointHandle,
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
+        wake_up: bool,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
+        if let Some(removed) = self.rb2mb.get(handle.0).copied() {
+            let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
+
+            // Remove the edge from the connectivity graph.
+            if let Some(parent_link) = multibody.link(removed.id).unwrap().parent_id() {
+                let parent_rb = multibody.link(parent_link).unwrap().rigid_body;
+                self.connectivity_graph.remove_edge(
+                    self.rb2mb.get(parent_rb.0).unwrap().graph_id,
+                    removed.graph_id,
+                );
+
+                if wake_up {
+                    islands.wake_up(bodies, RigidBodyHandle(handle.0), true);
+                    islands.wake_up(bodies, parent_rb, true);
+                }
+
+                // TODO: remove the node if it no longer has any attached edges?
+
+                // Extract the individual sub-trees generated by this removal.
+                let multibodies = multibody.remove_link(removed.id, true);
+
+                // Update the rb2mb mapping.
+                for multibody in multibodies {
+                    if multibody.num_links() == 1 {
+                        // We don’t have any multibody_joint attached to this body, remove it.
+                        if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) {
+                            self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id;
+                        }
+                    } else {
+                        let mb_id = self.multibodies.insert(multibody);
+                        for link in self.multibodies[mb_id].links() {
+                            let ids = self.rb2mb.get_mut(link.rigid_body.0).unwrap();
+                            ids.multibody = MultibodyIndex(mb_id);
+                            ids.id = link.internal_id;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    /// Removes all the multibody_joints from the multibody the given rigid-body is part of.
+    pub fn remove_multibody_articulations<Bodies>(
+        &mut self,
+        handle: RigidBodyHandle,
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
+        wake_up: bool,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
+        if let Some(removed) = self.rb2mb.get(handle.0).copied() {
+            // Remove the multibody.
+            let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
+            for link in multibody.links() {
+                let rb_handle = link.rigid_body;
+
+                if wake_up {
+                    islands.wake_up(bodies, rb_handle, true);
+                }
+
+                // Remove the rigid-body <-> multibody mapping for this link.
+                let removed = self.rb2mb.remove(rb_handle.0, Default::default()).unwrap();
+                // Remove the node (and all it’s edges) from the connectivity graph.
+                if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) {
+                    self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id;
+                }
+            }
+        }
+    }
+
+    pub fn remove_articulations_attached_to_rigid_body<Bodies>(
+        &mut self,
+        rb_to_remove: RigidBodyHandle,
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
+        // TODO: optimize this.
+        if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
+            let mut articulations_to_remove = vec![];
+            for (rb1, rb2, _) in self
+                .connectivity_graph
+                .interactions_with(link_to_remove.graph_id)
+            {
+                // There is an multibody_joint that we need to remove between these two bodies.
+                // If this is an outbound edge, then the multibody_joint’s handle is equal to the
+                // second body handle.
+                if rb1 == rb_to_remove {
+                    articulations_to_remove.push(MultibodyJointHandle(rb2.0));
+                } else {
+                    articulations_to_remove.push(MultibodyJointHandle(rb1.0));
+                }
+
+                islands.wake_up(bodies, rb1, true);
+                islands.wake_up(bodies, rb2, true);
+            }
+
+            for articulation_handle in articulations_to_remove {
+                self.remove(articulation_handle, islands, bodies, true);
+            }
+        }
+    }
+
+    /// Returns the link of this multibody attached to the given rigid-body.
+    ///
+    /// Returns `None` if `rb` isn’t part of any rigid-body.
+    pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyJointLink> {
+        self.rb2mb.get(rb.0)
+    }
+
+    /// Gets a reference to a multibody, based on its temporary index.
+    pub fn get_multibody(&self, index: MultibodyIndex) -> Option<&Multibody> {
+        self.multibodies.get(index.0)
+    }
+
+    /// Gets a mutable reference to a multibody, based on its temporary index.
+    ///
+    /// This method will bypass any modification-detection automatically done by the
+    /// `MultibodyJointSet`.
+    pub fn get_multibody_mut_internal(&mut self, index: MultibodyIndex) -> Option<&mut Multibody> {
+        self.multibodies.get_mut(index.0)
+    }
+
+    /// Gets a reference to the multibody identified by its `handle`.
+    pub fn get(&self, handle: MultibodyJointHandle) -> Option<(&Multibody, usize)> {
+        let link = self.rb2mb.get(handle.0)?;
+        let multibody = self.multibodies.get(link.multibody.0)?;
+        Some((multibody, link.id))
+    }
+
+    /// Gets a mutable reference to the multibody identified by its `handle`.
+    pub fn get_mut_internal(
+        &mut self,
+        handle: MultibodyJointHandle,
+    ) -> Option<(&mut Multibody, usize)> {
+        let link = self.rb2mb.get(handle.0)?;
+        let multibody = self.multibodies.get_mut(link.multibody.0)?;
+        Some((multibody, link.id))
+    }
+
+    /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
+    /// by an multibody_joint.
+    pub fn attached_bodies<'a>(
+        &'a self,
+        body: RigidBodyHandle,
+    ) -> impl Iterator<Item = RigidBodyHandle> + 'a {
+        self.rb2mb
+            .get(body.0)
+            .into_iter()
+            .flat_map(move |id| self.connectivity_graph.interactions_with(id.graph_id))
+            .map(move |inter| crate::utils::select_other((inter.0, inter.1), body))
+    }
+
+    /// Iterates through all the multibodies on this set.
+    pub fn multibodies(&self) -> impl Iterator<Item = &Multibody> {
+        self.multibodies.iter().map(|e| e.1)
+    }
+}
+
+impl Index<MultibodyIndex> for MultibodyJointSet {
+    type Output = Multibody;
+
+    fn index(&self, index: MultibodyIndex) -> &Multibody {
+        &self.multibodies[index.0]
+    }
+}
+
+// impl Index<MultibodyJointHandle> for MultibodyJointSet {
+//     type Output = Multibody;
+//
+//     fn index(&self, index: MultibodyJointHandle) -> &Multibody {
+//         &self.multibodies[index.0]
+//     }
+// }
diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs
new file mode 100644
index 0000000..94bd3de
--- /dev/null
+++ b/src/dynamics/joint/multibody_joint/multibody_link.rs
@@ -0,0 +1,173 @@
+use std::ops::{Deref, DerefMut};
+
+use crate::dynamics::{MultibodyJoint, RigidBodyHandle};
+use crate::math::{Isometry, Real};
+use crate::prelude::RigidBodyVelocity;
+
+pub(crate) struct KinematicState {
+    pub joint: MultibodyJoint,
+    pub parent_to_world: Isometry<Real>,
+    // TODO: should this be removed in favor of the rigid-body position?
+    pub local_to_world: Isometry<Real>,
+    pub local_to_parent: Isometry<Real>,
+}
+
+impl Clone for KinematicState {
+    fn clone(&self) -> Self {
+        Self {
+            joint: self.joint.clone(),
+            parent_to_world: self.parent_to_world.clone(),
+            local_to_world: self.local_to_world.clone(),
+            local_to_parent: self.local_to_parent.clone(),
+        }
+    }
+}
+
+/// One link of a multibody.
+pub struct MultibodyLink {
+    pub(crate) name: String,
+    // FIXME: make all those private.
+    pub(crate) internal_id: usize,
+    pub(crate) assembly_id: usize,
+    pub(crate) is_leaf: bool,
+
+    pub(crate) parent_internal_id: usize,
+    pub(crate) rigid_body: RigidBodyHandle,
+
+    /*
+     * Change at each time step.
+     */
+    pub(crate) state: KinematicState,
+
+    // FIXME: put this on a workspace buffer instead ?
+    pub(crate) velocity_dot_wrt_joint: RigidBodyVelocity,
+    pub(crate) velocity_wrt_joint: RigidBodyVelocity,
+}
+
+impl MultibodyLink {
+    /// Creates a new multibody link.
+    pub fn new(
+        rigid_body: RigidBodyHandle,
+        internal_id: usize,
+        assembly_id: usize,
+        parent_internal_id: usize,
+        joint: MultibodyJoint,
+        parent_to_world: Isometry<Real>,
+        local_to_world: Isometry<Real>,
+        local_to_parent: Isometry<Real>,
+    ) -> Self {
+        let is_leaf = true;
+        let velocity_dot_wrt_joint = RigidBodyVelocity::zero();
+        let velocity_wrt_joint = RigidBodyVelocity::zero();
+        let kinematic_state = KinematicState {
+            joint,
+            parent_to_world,
+            local_to_world,
+            local_to_parent,
+        };
+
+        MultibodyLink {
+            name: String::new(),
+            internal_id,
+            assembly_id,
+            is_leaf,
+            parent_internal_id,
+            state: kinematic_state,
+            velocity_dot_wrt_joint,
+            velocity_wrt_joint,
+            rigid_body,
+        }
+    }
+
+    pub fn joint(&self) -> &MultibodyJoint {
+        &self.state.joint
+    }
+
+    pub fn rigid_body_handle(&self) -> RigidBodyHandle {
+        self.rigid_body
+    }
+
+    /// Checks if this link is the root of the multibody.
+    #[inline]
+    pub fn is_root(&self) -> bool {
+        self.internal_id == 0
+    }
+
+    /// This link's name.
+    #[inline]
+    pub fn name(&self) -> &str {
+        &self.name
+    }
+
+    /// Sets this link's name.
+    #[inline]
+    pub fn set_name(&mut self, name: String) {
+        self.name = name
+    }
+
+    /// The handle of this multibody link.
+    #[inline]
+    pub fn link_id(&self) -> usize {
+        self.internal_id
+    }
+
+    /// The handle of the parent link.
+    #[inline]
+    pub fn parent_id(&self) -> Option<usize> {
+        if self.internal_id != 0 {
+            Some(self.parent_internal_id)
+        } else {
+            None
+        }
+    }
+
+    #[inline]
+    pub fn local_to_world(&self) -> &Isometry<Real> {
+        &self.state.local_to_world
+    }
+
+    #[inline]
+    pub fn local_to_parent(&self) -> &Isometry<Real> {
+        &self.state.local_to_parent
+    }
+}
+
+// FIXME: keep this even if we already have the Index2 traits?
+pub(crate) struct MultibodyLinkVec(pub Vec<MultibodyLink>);
+
+impl MultibodyLinkVec {
+    #[inline]
+    pub fn get_mut_with_parent(&mut self, i: usize) -> (&mut MultibodyLink, &MultibodyLink) {
+        let parent_id = self[i].parent_internal_id;
+
+        assert!(
+            parent_id != i,
+            "Internal error: circular rigid body dependency."
+        );
+        assert!(parent_id < self.len(), "Invalid parent index.");
+
+        unsafe {
+            let rb = &mut *(self.get_unchecked_mut(i) as *mut _);
+            let parent_rb = &*(self.get_unchecked(parent_id) as *const _);
+            (rb, parent_rb)
+        }
+    }
+}
+
+impl Deref for MultibodyLinkVec {
+    type Target = Vec<MultibodyLink>;
+
+    #[inline]
+    fn deref(&self) -> &Vec<MultibodyLink> {
+        let MultibodyLinkVec(ref me) = *self;
+        me
+    }
+}
+
+impl DerefMut for MultibodyLinkVec {
+    #[inline]
+    fn deref_mut(&mut self) -> &mut Vec<MultibodyLink> {
+        let MultibodyLinkVec(ref mut me) = *self;
+        me
+    }
+}
diff --git a/src/dynamics/joint/multibody_joint/multibody_workspace.rs b/src/dynamics/joint/multibody_joint/multibody_workspace.rs
new file mode 100644
index 0000000..767d751
--- /dev/null
+++ b/src/dynamics/joint/multibody_joint/multibody_workspace.rs
@@ -0,0 +1,27 @@
+use crate::dynamics::RigidBodyVelocity;
+use crate::math::Real;
+use na::DVector;
+
+/// A temporary workspace for various updates of the multibody.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
+pub(crate) struct MultibodyWorkspace {
+    pub accs: Vec<RigidBodyVelocity>,
+    pub ndofs_vec: DVector<Real>,
+}
+
+impl MultibodyWorkspace {
+    /// Create an empty workspace.
+    pub fn new() -> Self {
+        MultibodyWorkspace {
+            accs: Vec::new(),
+            ndofs_vec: DVector::zeros(0),
+        }
+    }
+
+    /// Resize the workspace so it is enough for `nlinks` links.
+    pub fn resize(&mut self, nlinks: usize, ndofs: usize) {
+        self.accs.resize(nlinks, RigidBodyVelocity::zero());
+        self.ndofs_vec = DVector::zeros(ndofs)
+    }
+}
diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
new file mode 100644
index 0000000..3367108
--- /dev/null
+++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
@@ -0,0 +1,122 @@
+#![allow(missing_docs)] // For downcast.
+
+use crate::dynamics::joint::MultibodyLink;
+use crate::dynamics::solver::{
+    AnyJointVelocityConstraint, JointGenericVelocityGroundConstraint, WritebackId,
+};
+use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
+use crate::math::Real;
+use na::DVector;
+
+/// Initializes and generate the velocity constraints applicable to the multibody links attached
+/// to this multibody_joint.
+pub fn unit_joint_limit_constraint(
+    params: &IntegrationParameters,
+    multibody: &Multibody,
+    link: &MultibodyLink,
+    limits: [Real; 2],
+    curr_pos: Real,
+    dof_id: usize,
+    j_id: &mut usize,
+    jacobians: &mut DVector<Real>,
+    constraints: &mut Vec<AnyJointVelocityConstraint>,
+) {
+    let ndofs = multibody.ndofs();
+    let joint_velocity = multibody.joint_velocity(link);
+
+    let min_enabled = curr_pos < limits[0];
+    let max_enabled = limits[1] < curr_pos;
+    let erp_inv_dt = params.erp_inv_dt();
+    let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
+    let rhs_wo_bias = joint_velocity[dof_id];
+
+    let dof_j_id = *j_id + dof_id + link.assembly_id;
+    jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
+    jacobians[dof_j_id] = 1.0;
+    jacobians[dof_j_id + ndofs] = 1.0;
+    multibody
+        .inv_augmented_mass()
+        .solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
+
+    let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
+    let impulse_bounds = [
+        min_enabled as u32 as Real * -Real::MAX,
+        max_enabled as u32 as Real * Real::MAX,
+    ];
+
+    let constraint = JointGenericVelocityGroundConstraint {
+        mj_lambda2: multibody.solver_id,
+        ndofs2: ndofs,
+        j_id2: *j_id,
+        joint_id: usize::MAX,
+        impulse: 0.0,
+        impulse_bounds,
+        inv_lhs: crate::utils::inv(lhs),
+        rhs: rhs_wo_bias + rhs_bias,
+        rhs_wo_bias,
+        writeback_id: WritebackId::Limit(dof_id),
+    };
+
+    constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
+        constraint,
+    ));
+    *j_id += 2 * ndofs;
+}
+
+/// Initializes and generate the velocity constraints applicable to the multibody links attached
+/// to this multibody_joint.
+pub fn unit_joint_motor_constraint(
+    params: &IntegrationParameters,
+    multibody: &Multibody,
+    link: &MultibodyLink,
+    motor: &JointMotor,
+    curr_pos: Real,
+    dof_id: usize,
+    j_id: &mut usize,
+    jacobians: &mut DVector<Real>,
+    constraints: &mut Vec<AnyJointVelocityConstraint>,
+) {
+    let ndofs = multibody.ndofs();
+    let joint_velocity = multibody.joint_velocity(link);
+
+    let motor_params = motor.motor_params(params.dt);
+
+    let dof_j_id = *j_id + dof_id + link.assembly_id;
+    jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
+    jacobians[dof_j_id] = 1.0;
+    jacobians[dof_j_id + ndofs] = 1.0;
+    multibody
+        .inv_augmented_mass()
+        .solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
+
+    let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
+    let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
+
+    let mut rhs_wo_bias = 0.0;
+    if motor_params.stiffness != 0.0 {
+        rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.stiffness;
+    }
+
+    if motor_params.damping != 0.0 {
+        let dvel = joint_velocity[dof_id];
+        rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
+    }
+
+    let constraint = JointGenericVelocityGroundConstraint {
+        mj_lambda2: multibody.solver_id,
+        ndofs2: ndofs,
+        j_id2: *j_id,
+        joint_id: usize::MAX,
+        impulse: 0.0,
+        impulse_bounds,
+        inv_lhs: crate::utils::inv(lhs),
+        rhs: rhs_wo_bias,
+        rhs_wo_bias,
+        writeback_id: WritebackId::Limit(dof_id),
+    };
+
+    constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
+        constraint,
+    ));
+    *j_id += 2 * ndofs;
+}
diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs
index 69edcb7..92fabde 100644
--- a/src/dynamics/joint/prismatic_joint.rs
+++ b/src/dynamics/joint/prismatic_joint.rs
@@ -1,250 +1,91 @@
-use crate::dynamics::SpringModel;
-use crate::math::{Isometry, Point, Real, Vector, DIM};
-use crate::utils::WBasis;
-use na::Unit;
-#[cfg(feature = "dim2")]
-use na::Vector2;
-#[cfg(feature = "dim3")]
-use na::Vector5;
+use crate::dynamics::joint::{JointAxesMask, JointData};
+use crate::dynamics::{JointAxis, MotorModel};
+use crate::math::{Point, Real, UnitVector};
 
-#[derive(Copy, Clone, PartialEq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// A joint that removes all relative motion between two bodies, except for the translations along one axis.
+#[derive(Copy, Clone, Debug, PartialEq)]
 pub struct PrismaticJoint {
-    /// Where the prismatic joint is attached on the first body, expressed in the local space of the first attached body.
-    pub local_anchor1: Point<Real>,
-    /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body.
-    pub local_anchor2: Point<Real>,
-    pub(crate) local_axis1: Unit<Vector<Real>>,
-    pub(crate) local_axis2: Unit<Vector<Real>>,
-    pub(crate) basis1: [Vector<Real>; DIM - 1],
-    pub(crate) basis2: [Vector<Real>; DIM - 1],
-
-    /// The impulse applied by this joint on the first body.
-    ///
-    /// The impulse applied to the second body is given by `-impulse`.
-    #[cfg(feature = "dim3")]
-    pub impulse: Vector5<Real>,
-    /// The impulse applied by this joint on the first body.
-    ///
-    /// The impulse applied to the second body is given by `-impulse`.
-    #[cfg(feature = "dim2")]
-    pub impulse: Vector2<Real>,
-
-    /// Whether or not this joint should enforce translational limits along its axis.
-    pub limits_enabled: bool,
-    /// The min an max relative position of the attached bodies along this joint's axis.
-    pub limits: [Real; 2],
-    /// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis.
-    ///
-    /// The impulse applied to the second body is given by `-impulse`.
-    pub limits_impulse: Real,
-
-    /// The target relative angular velocity the motor will attempt to reach.
-    pub motor_target_vel: Real,
-    /// The target relative angle along the joint axis the motor will attempt to reach.
-    pub motor_target_pos: Real,
-    /// The motor's stiffness.
-    /// See the documentation of `SpringModel` for more information on this parameter.
-    pub motor_stiffness: Real,
-    /// The motor's damping.
-    /// See the documentation of `SpringModel` for more information on this parameter.
-    pub motor_damping: Real,
-    /// The maximal impulse the motor is able to deliver.
-    pub motor_max_impulse: Real,
-    /// The angular impulse applied by the motor.
-    pub motor_impulse: Real,
-    /// The spring-like model used by the motor to reach the target velocity and .
-    pub motor_model: SpringModel,
+    data: JointData,
 }
 
 impl PrismaticJoint {
-    /// Creates a new prismatic joint with the given point of applications and axis, all expressed
-    /// in the local-space of the affected bodies.
-    #[cfg(feature = "dim2")]
-    pub fn new(
-        local_anchor1: Point<Real>,
-        local_axis1: Unit<Vector<Real>>,
-        local_anchor2: Point<Real>,
-        local_axis2: Unit<Vector<Real>>,
-    ) -> Self {
-        Self {
-            local_anchor1,
-            local_anchor2,
-            local_axis1,
-            local_axis2,
-            basis1: local_axis1.orthonormal_basis(),
-            basis2: local_axis2.orthonormal_basis(),
-            impulse: na::zero(),
-            limits_enabled: false,
-            limits: [-Real::MAX, Real::MAX],
-            limits_impulse: 0.0,
-            motor_target_vel: 0.0,
-            motor_target_pos: 0.0,
-            motor_stiffness: 0.0,
-            motor_damping: 0.0,
-            motor_max_impulse: Real::MAX,
-            motor_impulse: 0.0,
-            motor_model: SpringModel::VelocityBased,
-        }
+    pub fn new(axis: UnitVector<Real>) -> Self {
+        #[cfg(feature = "dim2")]
+        let mask = JointAxesMask::Y | JointAxesMask::ANG_X;
+        #[cfg(feature = "dim3")]
+        let mask = JointAxesMask::Y
+            | JointAxesMask::Z
+            | JointAxesMask::ANG_X
+            | JointAxesMask::ANG_Y
+            | JointAxesMask::ANG_Z;
+
+        let data = JointData::default()
+            .lock_axes(mask)
+            .local_axis1(axis)
+            .local_axis2(axis);
+        Self { data }
     }
 
-    /// Creates a new prismatic joint with the given point of applications and axis, all expressed
-    /// in the local-space of the affected bodies.
-    ///
-    /// The local tangent are vector orthogonal to the local axis. It is used to compute a basis orthonormal
-    /// to the joint's axis. If this tangent is set to zero, te orthonormal basis will be automatically
-    /// computed arbitrarily.
-    #[cfg(feature = "dim3")]
-    pub fn new(
-        local_anchor1: Point<Real>,
-        local_axis1: Unit<Vector<Real>>,
-        local_tangent1: Vector<Real>,
-        local_anchor2: Point<Real>,
-        local_axis2: Unit<Vector<Real>>,
-        local_tangent2: Vector<Real>,
-    ) -> Self {
-        let basis1 = if let Some(local_bitangent1) =
-            Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3)
-        {
-            [
-                local_bitangent1.cross(&local_axis1),
-                local_bitangent1.into_inner(),
-            ]
-        } else {
-            local_axis1.orthonormal_basis()
-        };
-
-        let basis2 = if let Some(local_bitangent2) =
-            Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3)
-        {
-            [
-                local_bitangent2.cross(&local_axis2),
-                local_bitangent2.into_inner(),
-            ]
-        } else {
-            local_axis2.orthonormal_basis()
-        };
-
-        Self {
-            local_anchor1,
-            local_anchor2,
-            local_axis1,
-            local_axis2,
-            basis1,
-            basis2,
-            impulse: na::zero(),
-            limits_enabled: false,
-            limits: [-Real::MAX, Real::MAX],
-            limits_impulse: 0.0,
-            motor_target_vel: 0.0,
-            motor_target_pos: 0.0,
-            motor_stiffness: 0.0,
-            motor_damping: 0.0,
-            motor_max_impulse: Real::MAX,
-            motor_impulse: 0.0,
-            motor_model: SpringModel::VelocityBased,
-        }
+    #[must_use]
+    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+        self.data = self.data.local_anchor1(anchor1);
+        self
     }
 
-    /// The local axis of this joint, expressed in the local-space of the first attached body.
-    pub fn local_axis1(&self) -> Unit<Vector<Real>> {
-        self.local_axis1
-    }
-
-    /// The local axis of this joint, expressed in the local-space of the second attached body.
-    pub fn local_axis2(&self) -> Unit<Vector<Real>> {
-        self.local_axis2
-    }
-
-    /// Can a SIMD constraint be used for resolving this joint?
-    pub fn supports_simd_constraints(&self) -> bool {
-        // SIMD revolute constraints don't support motors right now.
-        self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0)
-    }
-
-    // FIXME: precompute this?
-    #[cfg(feature = "dim2")]
-    pub(crate) fn local_frame1(&self) -> Isometry<Real> {
-        use na::{Matrix2, Rotation2, UnitComplex};
-
-        let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]);
-        let rotmat = Rotation2::from_matrix_unchecked(mat);
-        let rotation = UnitComplex::from_rotation_matrix(&rotmat);
-        let translation = self.local_anchor1.coords.into();
-        Isometry::from_parts(translation, rotation)
-    }
-
-    // FIXME: precompute this?
-    #[cfg(feature = "dim2")]
-    pub(crate) fn local_frame2(&self) -> Isometry<Real> {
-        use na::{Matrix2, Rotation2, UnitComplex};
-
-        let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]);
-        let rotmat = Rotation2::from_matrix_unchecked(mat);
-        let rotation = UnitComplex::from_rotation_matrix(&rotmat);
-        let translation = self.local_anchor2.coords.into();
-        Isometry::from_parts(translation, rotation)
-    }
-
-    // FIXME: precompute this?
-    #[cfg(feature = "dim3")]
-    pub(crate) fn local_frame1(&self) -> Isometry<Real> {
-        use na::{Matrix3, Rotation3, UnitQuaternion};
-
-        let mat = Matrix3::from_columns(&[
-            self.local_axis1.into_inner(),
-            self.basis1[0],
-            self.basis1[1],
-        ]);
-        let rotmat = Rotation3::from_matrix_unchecked(mat);
-        let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
-        let translation = self.local_anchor1.coords.into();
-        Isometry::from_parts(translation, rotation)
-    }
-
-    // FIXME: precompute this?
-    #[cfg(feature = "dim3")]
-    pub(crate) fn local_frame2(&self) -> Isometry<Real> {
-        use na::{Matrix3, Rotation3, UnitQuaternion};
-
-        let mat = Matrix3::from_columns(&[
-            self.local_axis2.into_inner(),
-            self.basis2[0],
-            self.basis2[1],
-        ]);
-        let rotmat = Rotation3::from_matrix_unchecked(mat);
-        let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
-        let translation = self.local_anchor2.coords.into();
-        Isometry::from_parts(translation, rotation)
+    #[must_use]
+    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+        self.data = self.data.local_anchor2(anchor2);
+        self
     }
 
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
-    pub fn configure_motor_model(&mut self, model: SpringModel) {
-        self.motor_model = model;
+    pub fn motor_model(mut self, model: MotorModel) -> Self {
+        self.data = self.data.motor_model(JointAxis::X, model);
+        self
     }
 
     /// Sets the target velocity this motor needs to reach.
-    pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
-        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
+    pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
+        self.data = self.data.motor_velocity(JointAxis::X, target_vel, factor);
+        self
     }
 
-    /// Sets the target position this motor needs to reach.
-    pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
-        self.configure_motor(target_pos, 0.0, stiffness, damping)
+    /// Sets the target angle this motor needs to reach.
+    pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
+        self.data = self
+            .data
+            .motor_position(JointAxis::X, target_pos, stiffness, damping);
+        self
     }
 
-    /// Configure both the target position and target velocity of the motor.
-    pub fn configure_motor(
-        &mut self,
+    /// Configure both the target angle and target velocity of the motor.
+    pub fn motor_axis(
+        mut self,
         target_pos: Real,
         target_vel: Real,
         stiffness: Real,
         damping: Real,
-    ) {
-        self.motor_target_vel = target_vel;
-        self.motor_target_pos = target_pos;
-        self.motor_stiffness = stiffness;
-        self.motor_damping = damping;
+    ) -> Self {
+        self.data = self
+            .data
+            .motor_axis(JointAxis::X, target_pos, target_vel, stiffness, damping);
+        self
+    }
+
+    pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self {
+        self.data = self.data.motor_max_impulse(JointAxis::X, max_impulse);
+        self
+    }
+
+    #[must_use]
+    pub fn limit_axis(mut self, limits: [Real; 2]) -> Self {
+        self.data = self.data.limit_axis(JointAxis::X, limits);
+        self
+    }
+}
+
+impl Into<JointData> for PrismaticJoint {
+    fn into(self) -> JointData {
+        self.data
     }
 }
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index 6531a89..9084c4d 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -1,174 +1,106 @@
-use crate::dynamics::SpringModel;
-use crate::math::{Isometry, Point, Real, Vector};
-use crate::utils::WBasis;
-use na::{RealField, Unit, Vector5};
+use crate::dynamics::joint::{JointAxesMask, JointData};
+use crate::dynamics::{JointAxis, MotorModel};
+use crate::math::{Point, Real};
+
+#[cfg(feature = "dim3")]
+use crate::math::UnitVector;
 
-#[derive(Copy, Clone, PartialEq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// A joint that removes all relative motion between two bodies, except for the rotations along one axis.
+#[derive(Copy, Clone, Debug, PartialEq)]
 pub struct RevoluteJoint {
-    /// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body.
-    pub local_anchor1: Point<Real>,
-    /// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body.
-    pub local_anchor2: Point<Real>,
-    /// The rotation axis of this revolute joint expressed in the local space of the first attached body.
-    pub local_axis1: Unit<Vector<Real>>,
-    /// The rotation axis of this revolute joint expressed in the local space of the second attached body.
-    pub local_axis2: Unit<Vector<Real>>,
-    /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
-    pub basis1: [Vector<Real>; 2],
-    /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
-    pub basis2: [Vector<Real>; 2],
-    /// The impulse applied by this joint on the first body.
-    ///
-    /// The impulse applied to the second body is given by `-impulse`.
-    pub impulse: Vector5<Real>,
-
-    /// Whether or not this joint should enforce translational limits along its axis.
-    pub limits_enabled: bool,
-    /// The min an max relative position of the attached bodies along this joint's axis.
-    pub limits: [Real; 2],
-    /// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis.
-    ///
-    /// The impulse applied to the second body is given by `-impulse`.
-    pub limits_impulse: Real,
-
-    /// The target relative angular velocity the motor will attempt to reach.
-    pub motor_target_vel: Real,
-    /// The target relative angle along the joint axis the motor will attempt to reach.
-    pub motor_target_pos: Real,
-    /// The motor's stiffness.
-    /// See the documentation of `SpringModel` for more information on this parameter.
-    pub motor_stiffness: Real,
-    /// The motor's damping.
-    /// See the documentation of `SpringModel` for more information on this parameter.
-    pub motor_damping: Real,
-    /// The maximal impulse the motor is able to deliver.
-    pub motor_max_impulse: Real,
-    /// The angular impulse applied by the motor.
-    pub motor_impulse: Real,
-    /// The spring-like model used by the motor to reach the target velocity and .
-    pub motor_model: SpringModel,
-
-    // Used to handle cases where the position target ends up being more than pi radians away.
-    pub(crate) motor_last_angle: Real,
-    // The angular impulse expressed in world-space.
-    pub(crate) world_ang_impulse: Vector<Real>,
-    // The world-space orientation of the free axis of the first attached body.
-    pub(crate) prev_axis1: Vector<Real>,
+    data: JointData,
 }
 
 impl RevoluteJoint {
-    /// Creates a new revolute joint with the given point of applications and axis, all expressed
-    /// in the local-space of the affected bodies.
-    pub fn new(
-        local_anchor1: Point<Real>,
-        local_axis1: Unit<Vector<Real>>,
-        local_anchor2: Point<Real>,
-        local_axis2: Unit<Vector<Real>>,
-    ) -> Self {
-        Self {
-            local_anchor1,
-            local_anchor2,
-            local_axis1,
-            local_axis2,
-            basis1: local_axis1.orthonormal_basis(),
-            basis2: local_axis2.orthonormal_basis(),
-            impulse: na::zero(),
-            world_ang_impulse: na::zero(),
-            limits_enabled: false,
-            limits: [-Real::MAX, Real::MAX],
-            limits_impulse: 0.0,
-            motor_target_vel: 0.0,
-            motor_target_pos: 0.0,
-            motor_stiffness: 0.0,
-            motor_damping: 0.0,
-            motor_max_impulse: Real::MAX,
-            motor_impulse: 0.0,
-            prev_axis1: *local_axis1,
-            motor_model: SpringModel::default(),
-            motor_last_angle: 0.0,
-        }
+    #[cfg(feature = "dim2")]
+    pub fn new() -> Self {
+        let mask = JointAxesMask::X | JointAxesMask::Y;
+
+        let data = JointData::default().lock_axes(mask);
+        Self { data }
     }
 
-    /// Can a SIMD constraint be used for resolving this joint?
-    pub fn supports_simd_constraints(&self) -> bool {
-        // SIMD revolute constraints don't support motors and limits right now.
-        !self.limits_enabled
-            && (self.motor_max_impulse == 0.0
-                || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
+    #[cfg(feature = "dim3")]
+    pub fn new(axis: UnitVector<Real>) -> Self {
+        let mask = JointAxesMask::X
+            | JointAxesMask::Y
+            | JointAxesMask::Z
+            | JointAxesMask::ANG_Y
+            | JointAxesMask::ANG_Z;
+
+        let data = JointData::default()
+            .lock_axes(mask)
+            .local_axis1(axis)
+            .local_axis2(axis);
+        Self { data }
+    }
+
+    pub fn data(&self) -> &JointData {
+        &self.data
+    }
+
+    #[must_use]
+    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+        self.data = self.data.local_anchor1(anchor1);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+        self.data = self.data.local_anchor2(anchor2);
+        self
     }
 
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
-    pub fn configure_motor_model(&mut self, model: SpringModel) {
-        self.motor_model = model;
+    pub fn motor_model(mut self, model: MotorModel) -> Self {
+        self.data = self.data.motor_model(JointAxis::AngX, model);
+        self
     }
 
     /// Sets the target velocity this motor needs to reach.
-    pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
-        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
+    pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
+        self.data = self
+            .data
+            .motor_velocity(JointAxis::AngX, target_vel, factor);
+        self
     }
 
     /// Sets the target angle this motor needs to reach.
-    pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
-        self.configure_motor(target_pos, 0.0, stiffness, damping)
+    pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
+        self.data = self
+            .data
+            .motor_position(JointAxis::AngX, target_pos, stiffness, damping);
+        self
     }
 
     /// Configure both the target angle and target velocity of the motor.
-    pub fn configure_motor(
-        &mut self,
+    pub fn motor_axis(
+        mut self,
         target_pos: Real,
         target_vel: Real,
         stiffness: Real,
         damping: Real,
-    ) {
-        self.motor_target_vel = target_vel;
-        self.motor_target_pos = target_pos;
-        self.motor_stiffness = stiffness;
-        self.motor_damping = damping;
+    ) -> Self {
+        self.data =
+            self.data
+                .motor_axis(JointAxis::AngX, target_pos, target_vel, stiffness, damping);
+        self
     }
 
-    /// Estimates the current position of the motor angle.
-    pub fn estimate_motor_angle(
-        &self,
-        body_pos1: &Isometry<Real>,
-        body_pos2: &Isometry<Real>,
-    ) -> Real {
-        Self::estimate_motor_angle_from_params(
-            &(body_pos1 * self.local_axis1),
-            &(body_pos1 * self.basis1[0]),
-            &(body_pos2 * self.basis2[0]),
-            self.motor_last_angle,
-        )
+    pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self {
+        self.data = self.data.motor_max_impulse(JointAxis::AngX, max_impulse);
+        self
     }
 
-    /// Estimates the current position of the motor angle given the joint parameters.
-    pub fn estimate_motor_angle_from_params(
-        axis1: &Unit<Vector<Real>>,
-        tangent1: &Vector<Real>,
-        tangent2: &Vector<Real>,
-        motor_last_angle: Real,
-    ) -> Real {
-        let last_angle_cycles = (motor_last_angle / Real::two_pi()).trunc() * Real::two_pi();
-
-        // Measure the position between 0 and 2-pi
-        let new_angle = if tangent1.cross(&tangent2).dot(&axis1) < 0.0 {
-            Real::two_pi() - tangent1.angle(&tangent2)
-        } else {
-            tangent1.angle(&tangent2)
-        };
-
-        // The last angle between 0 and 2-pi
-        let last_angle_zero_two_pi = motor_last_angle - last_angle_cycles;
-
-        // Figure out the smallest angle differance.
-        let mut angle_diff = new_angle - last_angle_zero_two_pi;
-        if angle_diff > Real::pi() {
-            angle_diff -= Real::two_pi()
-        } else if angle_diff < -Real::pi() {
-            angle_diff += Real::two_pi()
-        }
-
-        motor_last_angle + angle_diff
+    #[must_use]
+    pub fn limit_axis(mut self, limits: [Real; 2]) -> Self {
+        self.data = self.data.limit_axis(JointAxis::AngX, limits);
+        self
+    }
+}
+
+impl Into<JointData> for RevoluteJoint {
+    fn into(self) -> JointData {
+        self.data
     }
 }
diff --git a/src/dynamics/joint/spherical_joint.rs b/src/dynamics/joint/spherical_joint.rs
new file mode 100644
index 0000000..581b959
--- /dev/null
+++ b/src/dynamics/joint/spherical_joint.rs
@@ -0,0 +1,91 @@
+use crate::dynamics::joint::{JointAxesMask, JointData};
+use crate::dynamics::{JointAxis, MotorModel};
+use crate::math::{Point, Real};
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct SphericalJoint {
+    data: JointData,
+}
+
+impl SphericalJoint {
+    pub fn new() -> Self {
+        let data =
+            JointData::default().lock_axes(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z);
+        Self { data }
+    }
+
+    pub fn data(&self) -> &JointData {
+        &self.data
+    }
+
+    #[must_use]
+    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+        self.data = self.data.local_anchor1(anchor1);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+        self.data = self.data.local_anchor2(anchor2);
+        self
+    }
+
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
+        self.data = self.data.motor_model(axis, model);
+        self
+    }
+
+    /// Sets the target velocity this motor needs to reach.
+    pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
+        self.data = self.data.motor_velocity(axis, target_vel, factor);
+        self
+    }
+
+    /// Sets the target angle this motor needs to reach.
+    pub fn motor_position(
+        mut self,
+        axis: JointAxis,
+        target_pos: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> Self {
+        self.data = self
+            .data
+            .motor_position(axis, target_pos, stiffness, damping);
+        self
+    }
+
+    /// Configure both the target angle and target velocity of the motor.
+    pub fn motor_axis(
+        mut self,
+        axis: JointAxis,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> Self {
+        self.data = self
+            .data
+            .motor_axis(axis, target_pos, target_vel, stiffness, damping);
+        self
+    }
+
+    pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self {
+        self.data = self.data.motor_max_impulse(axis, max_impulse);
+        self
+    }
+
+    #[must_use]
+    pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
+        self.data = self.data.limit_axis(axis, limits);
+        self
+    }
+}
+
+impl Into<JointData> for SphericalJoint {
+    fn into(self) -> JointData {
+        self.data
+    }
+}
diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 825fa49..65c294c 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -1,4 +1,4 @@
-//! Structures related to dynamics: bodies, joints, etc.
+//! Structures related to dynamics: bodies, impulse_joints, etc.
 
 pub use self::ccd::CCDSolver;
 pub use self::coefficient_combine_rule::CoefficientCombineRule;
@@ -6,18 +6,7 @@ pub use self::integration_parameters::IntegrationParameters;
 pub use self::island_manager::IslandManager;
 pub(crate) use self::joint::JointGraphEdge;
 pub(crate) use self::joint::JointIndex;
-#[cfg(feature = "dim3")]
-pub use self::joint::RevoluteJoint;
-pub use self::joint::{
-    BallJoint,
-    FixedJoint,
-    Joint,
-    JointHandle,
-    JointParams,
-    JointSet,
-    PrismaticJoint,
-    SpringModel, // GenericJoint
-};
+pub use self::joint::*;
 pub use self::rigid_body_components::*;
 #[cfg(not(feature = "parallel"))]
 pub(crate) use self::solver::IslandSolver;
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index a135a1c..e24cb57 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -4,9 +4,11 @@ use crate::geometry::{
     ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition,
     ColliderShape,
 };
-use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Translation, Vector};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
+};
 use crate::parry::partitioning::IndexedData;
-use crate::utils::{WCross, WDot};
+use crate::utils::{WAngularInertia, WCross, WDot};
 use num::Zero;
 
 /// The unique handle of a rigid body added to a `RigidBodySet`.
@@ -276,6 +278,13 @@ impl RigidBodyMassProps {
         crate::utils::inv(self.effective_inv_mass)
     }
 
+    /// The effective world-space angular inertia (that takes the potential rotation locking into account) of
+    /// this rigid-body.
+    #[must_use]
+    pub fn effective_angular_inertia(&self) -> AngularInertia<Real> {
+        self.effective_world_inv_inertia_sqrt.squared().inverse()
+    }
+
     /// Update the world-space mass properties of `self`, taking into account the new position.
     pub fn update_world_mass_properties(&mut self, position: &Isometry<Real>) {
         self.world_com = self.local_mprops.world_com(&position);
@@ -348,6 +357,51 @@ impl Default for RigidBodyVelocity {
 }
 
 impl RigidBodyVelocity {
+    /// Create a new rigid-body velocity component.
+    #[must_use]
+    pub fn new(linvel: Vector<Real>, angvel: AngVector<Real>) -> Self {
+        Self { linvel, angvel }
+    }
+
+    /// Converts a slice to a rigid-body velocity.
+    ///
+    /// The slice must contain at least 3 elements: the `slice[0..2]  contains
+    /// the linear velocity and the `slice[2]` contains the angular velocity.
+    #[must_use]
+    #[cfg(feature = "dim2")]
+    pub fn from_slice(slice: &[Real]) -> Self {
+        Self {
+            linvel: Vector::new(slice[0], slice[1]),
+            angvel: slice[2],
+        }
+    }
+
+    /// Converts a slice to a rigid-body velocity.
+    ///
+    /// The slice must contain at least 6 elements: the `slice[0..3]  contains
+    /// the linear velocity and the `slice[3..6]` contains the angular velocity.
+    #[must_use]
+    #[cfg(feature = "dim3")]
+    pub fn from_slice(slice: &[Real]) -> Self {
+        Self {
+            linvel: Vector::new(slice[0], slice[1], slice[2]),
+            angvel: AngVector::new(slice[3], slice[4], slice[5]),
+        }
+    }
+
+    #[cfg(feature = "dim2")]
+    pub(crate) fn from_vectors(linvel: Vector<Real>, angvel: na::Vector1<Real>) -> Self {
+        Self {
+            linvel,
+            angvel: angvel.x,
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn from_vectors(linvel: Vector<Real>, angvel: Vector<Real>) -> Self {
+        Self { linvel, angvel }
+    }
+
     /// Velocities set to zero.
     #[must_use]
     pub fn zero() -> Self {
@@ -357,6 +411,82 @@ impl RigidBodyVelocity {
         }
     }
 
+    /// This velocity seen as a slice.
+    ///
+    /// The linear part is stored first.
+    #[inline]
+    pub fn as_slice(&self) -> &[Real] {
+        self.as_vector().as_slice()
+    }
+
+    /// This velocity seen as a mutable slice.
+    ///
+    /// The linear part is stored first.
+    #[inline]
+    pub fn as_mut_slice(&mut self) -> &mut [Real] {
+        self.as_vector_mut().as_mut_slice()
+    }
+
+    /// This velocity seen as a vector.
+    ///
+    /// The linear part is stored first.   
+    #[inline]
+    #[cfg(feature = "dim2")]
+    pub fn as_vector(&self) -> &na::Vector3<Real> {
+        unsafe { std::mem::transmute(self) }
+    }
+
+    /// This velocity seen as a mutable vector.
+    ///
+    /// The linear part is stored first.    
+    #[inline]
+    #[cfg(feature = "dim2")]
+    pub fn as_vector_mut(&mut self) -> &mut na::Vector3<Real> {
+        unsafe { std::mem::transmute(self) }
+    }
+
+    /// This velocity seen as a vector.
+    ///
+    /// The linear part is stored first.   
+    #[inline]
+    #[cfg(feature = "dim3")]
+    pub fn as_vector(&self) -> &na::Vector6<Real> {
+        unsafe { std::mem::transmute(self) }
+    }
+
+    /// This velocity seen as a mutable vector.
+    ///
+    /// The linear part is stored first.    
+    #[inline]
+    #[cfg(feature = "dim3")]
+    pub fn as_vector_mut(&mut self) -> &mut na::Vector6<Real> {
+        unsafe { std::mem::transmute(self) }
+    }
+
+    /// Return `self` transformed by `transform`.
+    #[must_use]
+    pub fn transformed(self, transform: &Isometry<Real>) -> Self {
+        Self {
+            linvel: transform * self.linvel,
+            #[cfg(feature = "dim2")]
+            angvel: self.angvel,
+            #[cfg(feature = "dim3")]
+            angvel: transform * self.angvel,
+        }
+    }
+
+    /// Return `self` rotated by `rotation`.
+    #[must_use]
+    pub fn rotated(self, rotation: &Rotation<Real>) -> Self {
+        Self {
+            linvel: rotation * self.linvel,
+            #[cfg(feature = "dim2")]
+            angvel: self.angvel,
+            #[cfg(feature = "dim3")]
+            angvel: rotation * self.angvel,
+        }
+    }
+
     /// The approximate kinetic energy of this rigid-body.
     ///
     /// This approximation does not take the rigid-body's mass and angular inertia
@@ -471,6 +601,38 @@ impl RigidBodyVelocity {
     }
 }
 
+impl std::ops::Mul<Real> for RigidBodyVelocity {
+    type Output = Self;
+
+    #[must_use]
+    fn mul(self, rhs: Real) -> Self {
+        RigidBodyVelocity {
+            linvel: self.linvel * rhs,
+            angvel: self.angvel * rhs,
+        }
+    }
+}
+
+impl std::ops::Add<RigidBodyVelocity> for RigidBodyVelocity {
+    type Output = Self;
+
+    #[must_use]
+    fn add(self, rhs: Self) -> Self {
+        RigidBodyVelocity {
+            linvel: self.linvel + rhs.linvel,
+            angvel: self.angvel + rhs.angvel,
+        }
+    }
+}
+
+impl std::ops::AddAssign<RigidBodyVelocity> for RigidBodyVelocity {
+    #[must_use]
+    fn add_assign(&mut self, rhs: Self) {
+        self.linvel += rhs.linvel;
+        self.angvel += rhs.angvel;
+    }
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone, Debug, Copy, PartialEq)]
 /// Damping factors to progressively slow down a rigid-body.
@@ -784,7 +946,7 @@ impl Default for RigidBodyActivation {
 }
 
 impl RigidBodyActivation {
-    /// The default amount of energy bellow which a body can be put to sleep by nphysics.
+    /// The default amount of energy bellow which a body can be put to sleep by rapier.
     pub fn default_threshold() -> Real {
         0.01
     }
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 607f7ff..34f7bdf 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -1,11 +1,11 @@
 use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption};
 use crate::dynamics::{
-    IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,
-    RigidBodyType,
+    ImpulseJointSet, RigidBody, RigidBodyCcd, RigidBodyChanges, RigidBodyDamping, RigidBodyForces,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::dynamics::{
-    JointSet, RigidBody, RigidBodyCcd, RigidBodyChanges, RigidBodyDamping, RigidBodyForces,
-    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
+    IslandManager, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance,
+    RigidBodyHandle, RigidBodyType,
 };
 use crate::geometry::ColliderSet;
 use std::ops::{Index, IndexMut};
@@ -132,13 +132,14 @@ impl RigidBodySet {
         handle
     }
 
-    /// Removes a rigid-body, and all its attached colliders and joints, from these sets.
+    /// Removes a rigid-body, and all its attached colliders and impulse_joints, from these sets.
     pub fn remove(
         &mut self,
         handle: RigidBodyHandle,
         islands: &mut IslandManager,
         colliders: &mut ColliderSet,
-        joints: &mut JointSet,
+        impulse_joints: &mut ImpulseJointSet,
+        multibody_joints: &mut MultibodyJointSet,
     ) -> Option<RigidBody> {
         let rb = self.bodies.remove(handle.0)?;
         /*
@@ -154,9 +155,10 @@ impl RigidBodySet {
         }
 
         /*
-         * Remove joints attached to this rigid-body.
+         * Remove impulse_joints attached to this rigid-body.
          */
-        joints.remove_joints_attached_to_rigid_body(handle, islands, self);
+        impulse_joints.remove_joints_attached_to_rigid_body(handle, islands, self);
+        multibody_joints.remove_articulations_attached_to_rigid_body(handle, islands, self);
 
         Some(rb)
     }
diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index 366a5b3..c5b2601 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/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);
diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs
index b457020..9160f2e 100644
--- a/src/dynamics/solver/delta_vel.rs
+++ b/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 {
diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs
new file mode 100644
index 0000000..fb06335
--- /dev/null
+++ b/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();
+    }
+}
diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs
new file mode 100644
index 0000000..150be8b
--- /dev/null
+++ b/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;
+            }
+        }
+    }
+}
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index c6baea1..67cc805 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/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 =
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index 912fe1d..a862480 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/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();
         }
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_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;
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
deleted file mode 100644
index ee465cd..0000000
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
+++ /dev/null
@@ -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)
-            }
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
deleted file mode 100644
index 3ab13f7..0000000
--- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
+++ /dev/null
@@ -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;
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs
deleted file mode 100644
index 0c0a6fd..0000000
--- a/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs
+++ /dev/null
@@ -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);
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
deleted file mode 100644
index 8bfc1a6..0000000
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
+++ /dev/null
@@ -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;
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
deleted file mode 100644
index 0421d49..0000000
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
+++ /dev/null
@@ -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)
-            }
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
deleted file mode 100644
index a7b5ea0..0000000
--- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
+++ /dev/null
@@ -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;
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
deleted file mode 100644
index d44c761..0000000
--- a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
+++ /dev/null
@@ -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);
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
deleted file mode 100644
index 1eb04a9..0000000
--- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
+++ /dev/null
@@ -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)
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs
deleted file mode 100644
index 2751373..0000000
--- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs
+++ /dev/null
@@ -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)
-            }
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
index d723387..f42038c 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ b/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!(),
diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
new file mode 100644
index 0000000..2646fe8
--- /dev/null
+++ b/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;
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
new file mode 100644
index 0000000..92bcc9f
--- /dev/null
+++ b/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;
+            }
+        }
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
deleted file mode 100644
index 56e19fc..0000000
--- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
+++ /dev/null
@@ -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!(),
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
new file mode 100644
index 0000000..3e31256
--- /dev/null
+++ b/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],
+            }
+        }
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
new file mode 100644
index 0000000..dd17ecf
--- /dev/null
+++ b/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;
+            }
+        }
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs
index 9196e69..503bd81 100644
--- a/src/dynamics/solver/joint_constraint/mod.rs
+++ b/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;
diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
deleted file mode 100644
index d68f7ef..0000000
--- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
+++ /dev/null
@@ -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;
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs
deleted file mode 100644
index 95c1bcb..0000000
--- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs
+++ /dev/null
@@ -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);
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
deleted file mode 100644
index 4e94c79..0000000
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
+++ /dev/null
@@ -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;
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
deleted file mode 100644
index 984adcb..0000000
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
+++ /dev/null
@@ -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);
-            }
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
deleted file mode 100644
index ad470a4..0000000
--- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
+++ /dev/null
@@ -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;
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs
deleted file mode 100644
index 5881cb3..0000000
--- a/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs
+++ /dev/null
@@ -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);
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
deleted file mode 100644
index ca15cd9..0000000
--- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
+++ /dev/null
@@ -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;
-        }
-    }
-}
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
deleted file mode 100644
index 6e65b76..0000000
--- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
+++ /dev/null
@@ -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)
-            }
-        }
-    }
-}
diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs
index 33b6f9a..7607c28 100644
--- a/src/dynamics/solver/mod.rs
+++ b/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;
diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs
index bd6fc5d..e695b3d 100644
--- a/src/dynamics/solver/parallel_island_solver.rs
+++ b/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);
 
diff --git a/src/dynamics/solver/parallel_position_solver.rs b/src/dynamics/solver/parallel_position_solver.rs
deleted file mode 100644
index ec480f5..0000000
--- a/src/dynamics/solver/parallel_position_solver.rs
+++ /dev/null
@@ -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();
-            }
-        }
-    }
-}
diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs
index 6b00b73..3871731 100644
--- a/src/dynamics/solver/parallel_solver_constraints.rs
+++ b/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;
                     }
                 }
             }
         }
+
+         */
     }
 }
diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs
index 54792f8..69ceb03 100644
--- a/src/dynamics/solver/parallel_velocity_solver.rs
+++ b/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();
             }
diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs
deleted file mode 100644
index dca0655..0000000
--- a/src/dynamics/solver/position_constraint.rs
+++ /dev/null
@@ -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;
-    }
-}
diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs
deleted file mode 100644
index 0b8e762..0000000
--- a/src/dynamics/solver/position_constraint_wide.rs
+++ /dev/null
@@ -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);
-        }
-    }
-}
diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
deleted file mode 100644
index 53df7f8..0000000
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ /dev/null
@@ -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;
-    }
-}
diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs
deleted file mode 100644
index 7d4ff96..0000000
--- a/src/dynamics/solver/position_ground_constraint_wide.rs
+++ /dev/null
@@ -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);
-        }
-    }
-}
diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs
deleted file mode 100644
index 48b71aa..0000000
--- a/src/dynamics/solver/position_solver.rs
+++ /dev/null
@@ -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);
-        }
-    }
-}
diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs
index a9aa780..cfd7575 100644
--- a/src/dynamics/solver/solver_constraints.rs
+++ b/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,
+            );
         }
     }
 }
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 0d82f81..719468d 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/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]
 }
diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs
index cb6d476..406f68e 100644
--- a/src/dynamics/solver/velocity_constraint_element.rs
+++ b/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);
+            }
         }
     }
 }
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index baaf643..0e2e36a 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/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;
+        }
+    }
 }
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index d1d5e8c..87865b3 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/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;
+        }
+    }
 }
diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index 2d4d633..adfcfda 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/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);
+            }
         }
     }
 }
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 3aede0b..e1ea8f6 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/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;
+        }
+    }
 }
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index 9ceb9d9..9ecdbfb 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/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);
+        }
     }
 }
diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs
index 0ed12b6..f4b9fa1 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs
@@ -622,7 +622,7 @@ impl BroadPhase {
 
 #[cfg(test)]
 mod test {
-    use crate::dynamics::{IslandManager, JointSet, RigidBodyBuilder, RigidBodySet};
+    use crate::dynamics::{IslandManager, ImpulseJointSet, RigidBodyBuilder, RigidBodySet};
     use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet};
 
     #[test]
@@ -630,7 +630,7 @@ mod test {
         let mut broad_phase = BroadPhase::new();
         let mut bodies = RigidBodySet::new();
         let mut colliders = ColliderSet::new();
-        let mut joints = JointSet::new();
+        let mut impulse_joints = ImpulseJointSet::new();
         let mut islands = IslandManager::new();
 
         let rb = RigidBodyBuilder::new_dynamic().build();
@@ -641,7 +641,7 @@ mod test {
         let mut events = Vec::new();
         broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
 
-        bodies.remove(hrb, &mut islands, &mut colliders, &mut joints);
+        bodies.remove(hrb, &mut islands, &mut colliders, &mut impulse_joints);
         broad_phase.update(0.0, &mut colliders, &[], &[coh], &mut events);
 
         // Create another body.
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index f4e7834..92b7800 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -35,8 +35,6 @@ pub struct ContactData {
     /// collider's rigid-body.
     #[cfg(feature = "dim3")]
     pub tangent_impulse: na::Vector2<Real>,
-    /// The target velocity correction at the contact point.
-    pub rhs: Real,
 }
 
 impl Default for ContactData {
@@ -44,7 +42,6 @@ impl Default for ContactData {
         Self {
             impulse: 0.0,
             tangent_impulse: na::zero(),
-            rhs: 0.0,
         }
     }
 }
@@ -119,12 +116,9 @@ pub struct ContactManifoldData {
     pub rigid_body1: Option<RigidBodyHandle>,
     /// The second rigid-body involved in this contact manifold.
     pub rigid_body2: Option<RigidBodyHandle>,
-    pub(crate) warmstart_multiplier: Real,
     // The two following are set by the constraints solver.
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     pub(crate) constraint_index: usize,
-    #[cfg_attr(feature = "serde-serialize", serde(skip))]
-    pub(crate) position_constraint_index: usize,
     // We put the following fields here to avoids reading the colliders inside of the
     // contact preparation method.
     /// Flags used to control some aspects of the constraints solver for this contact manifold.
@@ -177,26 +171,15 @@ pub struct SolverContact {
     /// This is set to zero by default. Set to a non-zero value to
     /// simulate, e.g., conveyor belts.
     pub tangent_velocity: Vector<Real>,
-    /// The warmstart impulse, along the contact normal, applied by this contact to the first collider's rigid-body.
-    pub warmstart_impulse: Real,
-    /// The warmstart friction impulse along the vector orthonormal to the contact normal, applied to the first
-    /// collider's rigid-body.
-    #[cfg(feature = "dim2")]
-    pub warmstart_tangent_impulse: Real,
-    /// The warmstart friction impulses along the basis orthonormal to the contact normal, applied to the first
-    /// collider's rigid-body.
-    #[cfg(feature = "dim3")]
-    pub warmstart_tangent_impulse: na::Vector2<Real>,
-    /// The last velocity correction targeted by this contact.
-    pub prev_rhs: Real,
+    /// Whether or not this contact existed during the last timestep.
+    pub is_new: bool,
 }
 
 impl SolverContact {
     /// Should we treat this contact as a bouncy contact?
     /// If `true`, use [`Self::restitution`].
     pub fn is_bouncy(&self) -> bool {
-        let is_new = self.warmstart_impulse == 0.0;
-        if is_new {
+        if self.is_new {
             // Treat new collisions as bouncing at first, unless we have zero restitution.
             self.restitution > 0.0
         } else {
@@ -222,9 +205,7 @@ impl ContactManifoldData {
         Self {
             rigid_body1,
             rigid_body2,
-            warmstart_multiplier: Self::min_warmstart_multiplier(),
             constraint_index: 0,
-            position_constraint_index: 0,
             solver_flags,
             normal: Vector::zeros(),
             solver_contacts: Vec::new(),
@@ -239,34 +220,4 @@ impl ContactManifoldData {
     pub fn num_active_contacts(&self) -> usize {
         self.solver_contacts.len()
     }
-
-    pub(crate) fn min_warmstart_multiplier() -> Real {
-        // Multiplier used to reduce the amount of warm-starting.
-        // This coefficient increases exponentially over time, until it reaches 1.0.
-        // This will reduce significant overshoot at the timesteps that
-        // follow a timestep involving high-velocity impacts.
-        1.0 // 0.01
-    }
-
-    // pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) {
-    //     // In 2D, tall stacks will actually suffer from this
-    //     // because oscillation due to inaccuracies in 2D often
-    //     // cause contacts to break, which would result in
-    //     // a reset of the warmstart multiplier.
-    //     if cfg!(feature = "dim2") {
-    //         manifold.data.warmstart_multiplier = 1.0;
-    //         return;
-    //     }
-    //
-    //     for pt in &manifold.points {
-    //         if pt.data.impulse != 0.0 {
-    //             manifold.data.warmstart_multiplier =
-    //                 (manifold.data.warmstart_multiplier * 2.0).min(1.0);
-    //             return;
-    //         }
-    //     }
-    //
-    //     // Reset the multiplier.
-    //     manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier()
-    // }
 }
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 82ee99d..643b584 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -968,9 +968,7 @@ impl NarrowPhase {
                             friction,
                             restitution,
                             tangent_velocity: Vector::zeros(),
-                            warmstart_impulse: contact.data.impulse,
-                            warmstart_tangent_impulse: contact.data.tangent_impulse,
-                            prev_rhs: contact.data.rhs,
+                            is_new: contact.data.impulse == 0.0,
                         };
 
                         manifold.data.solver_contacts.push(solver_contact);
@@ -1027,7 +1025,7 @@ impl NarrowPhase {
     }
 
     /// Retrieve all the interactions with at least one contact point, happening between two active bodies.
-    // NOTE: this is very similar to the code from JointSet::select_active_interactions.
+    // NOTE: this is very similar to the code from ImpulseJointSet::select_active_interactions.
     pub(crate) fn select_active_contacts<'a, Bodies>(
         &'a mut self,
         islands: &IslandManager,
diff --git a/src/lib.rs b/src/lib.rs
index 57e1f0d..515384d 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -11,7 +11,7 @@
 //! User documentation for Rapier is on [the official Rapier site](https://rapier.rs/docs/).
 
 #![deny(bare_trait_objects)]
-#![warn(missing_docs)]
+#![allow(missing_docs)] // FIXME: deny that
 
 #[cfg(all(feature = "dim2", feature = "f32"))]
 pub extern crate parry2d as parry;
@@ -140,16 +140,64 @@ pub mod utils;
 /// Elementary mathematical entities (vectors, matrices, isometries, etc).
 pub mod math {
     pub use parry::math::*;
+
+    /*
+     * 2D
+     */
     /// Max number of pairs of contact points from the same
     /// contact manifold that can be solved as part of a
     /// single contact constraint.
     #[cfg(feature = "dim2")]
     pub const MAX_MANIFOLD_POINTS: usize = 2;
+
+    /// The type of a constraint Jacobian in twist coordinates.
+    #[cfg(feature = "dim2")]
+    pub type Jacobian<N> = na::Matrix3xX<N>;
+
+    /// The type of a slice of the constraint Jacobian in twist coordinates.
+    #[cfg(feature = "dim2")]
+    pub type JacobianSlice<'a, N> = na::MatrixSlice3xX<'a, N>;
+
+    /// The type of a mutable slice of the constraint Jacobian in twist coordinates.
+    #[cfg(feature = "dim2")]
+    pub type JacobianSliceMut<'a, N> = na::MatrixSliceMut3xX<'a, N>;
+
+    /// The maximum number of possible rotations and translations of a rigid body.
+    #[cfg(feature = "dim2")]
+    pub const SPATIAL_DIM: usize = 3;
+
+    /// The maximum number of rotational degrees of freedom of a rigid-body.
+    #[cfg(feature = "dim2")]
+    pub const ANG_DIM: usize = 1;
+
+    /*
+     * 3D
+     */
     /// Max number of pairs of contact points from the same
     /// contact manifold that can be solved as part of a
     /// single contact constraint.
     #[cfg(feature = "dim3")]
     pub const MAX_MANIFOLD_POINTS: usize = 4;
+
+    /// The type of a constraint Jacobian in twist coordinates.
+    #[cfg(feature = "dim3")]
+    pub type Jacobian<N> = na::Matrix6xX<N>;
+
+    /// The type of a slice of the constraint Jacobian in twist coordinates.
+    #[cfg(feature = "dim3")]
+    pub type JacobianSlice<'a, N> = na::MatrixSlice6xX<'a, N>;
+
+    /// The type of a mutable slice of the constraint Jacobian in twist coordinates.
+    #[cfg(feature = "dim3")]
+    pub type JacobianSliceMut<'a, N> = na::MatrixSliceMut6xX<'a, N>;
+
+    /// The maximum number of possible rotations and translations of a rigid body.
+    #[cfg(feature = "dim3")]
+    pub const SPATIAL_DIM: usize = 6;
+
+    /// The maximum number of rotational degrees of freedom of a rigid-body.
+    #[cfg(feature = "dim3")]
+    pub const ANG_DIM: usize = 3;
 }
 
 /// Prelude containing the common types defined by Rapier.
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 4ba8bfa..b244860 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -5,10 +5,10 @@ use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 #[cfg(not(feature = "parallel"))]
 use crate::dynamics::IslandSolver;
 use crate::dynamics::{
-    CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodyActivation, RigidBodyCcd,
-    RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces,
-    RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
-    RigidBodyVelocity,
+    CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
+    RigidBodyActivation, RigidBodyCcd, RigidBodyChanges, RigidBodyColliders, RigidBodyDamping,
+    RigidBodyDominance, RigidBodyForces, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps,
+    RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
 };
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
@@ -155,60 +155,6 @@ impl PhysicsPipeline {
         self.counters.stages.collision_detection_time.pause();
     }
 
-    fn solve_position_constraints<Bodies>(
-        &mut self,
-        integration_parameters: &IntegrationParameters,
-        islands: &IslandManager,
-        bodies: &mut Bodies,
-    ) where
-        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
-    {
-        #[cfg(not(feature = "parallel"))]
-        {
-            enable_flush_to_zero!();
-
-            for island_id in 0..islands.num_islands() {
-                self.solvers[island_id].solve_position_constraints(
-                    island_id,
-                    islands,
-                    &mut self.counters,
-                    integration_parameters,
-                    bodies,
-                )
-            }
-        }
-
-        #[cfg(feature = "parallel")]
-        {
-            use rayon::prelude::*;
-            use std::sync::atomic::Ordering;
-
-            let num_islands = islands.num_islands();
-            let solvers = &mut self.solvers[..num_islands];
-            let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _);
-
-            rayon::scope(|scope| {
-                enable_flush_to_zero!();
-
-                solvers
-                    .par_iter_mut()
-                    .enumerate()
-                    .for_each(|(island_id, solver)| {
-                        let bodies: &mut Bodies =
-                            unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
-
-                        solver.solve_position_constraints(
-                            scope,
-                            island_id,
-                            islands,
-                            integration_parameters,
-                            bodies,
-                        )
-                    });
-            });
-        }
-    }
-
     fn build_islands_and_solve_velocity_constraints<Bodies, Colliders>(
         &mut self,
         gravity: &Vector<Real>,
@@ -217,7 +163,8 @@ impl PhysicsPipeline {
         narrow_phase: &mut NarrowPhase,
         bodies: &mut Bodies,
         colliders: &mut Colliders,
-        joints: &mut JointSet,
+        impulse_joints: &mut ImpulseJointSet,
+        multibody_joints: &mut MultibodyJointSet,
     ) where
         Bodies: ComponentSetMut<RigidBodyPosition>
             + ComponentSetMut<RigidBodyVelocity>
@@ -235,7 +182,8 @@ impl PhysicsPipeline {
             bodies,
             colliders,
             narrow_phase,
-            joints,
+            impulse_joints,
+            multibody_joints,
             integration_parameters.min_island_size,
         );
         self.counters.stages.island_construction_time.pause();
@@ -257,7 +205,11 @@ impl PhysicsPipeline {
             &mut manifolds,
             &mut self.manifold_indices,
         );
-        joints.select_active_interactions(islands, bodies, &mut self.joint_constraint_indices);
+        impulse_joints.select_active_interactions(
+            islands,
+            bodies,
+            &mut self.joint_constraint_indices,
+        );
 
         self.counters.stages.update_time.resume();
         for handle in islands.active_dynamic_bodies() {
@@ -274,6 +226,13 @@ impl PhysicsPipeline {
                 forces.add_gravity_acceleration(&gravity, effective_inv_mass)
             });
         }
+
+        for multibody in &mut multibody_joints.multibodies {
+            multibody
+                .1
+                .update_dynamics(integration_parameters.dt, bodies);
+            multibody.1.update_acceleration(bodies);
+        }
         self.counters.stages.update_time.pause();
 
         self.counters.stages.solver_time.resume();
@@ -287,7 +246,7 @@ impl PhysicsPipeline {
             enable_flush_to_zero!();
 
             for island_id in 0..islands.num_islands() {
-                self.solvers[island_id].init_constraints_and_solve_velocity_constraints(
+                self.solvers[island_id].init_and_solve(
                     island_id,
                     &mut self.counters,
                     integration_parameters,
@@ -295,8 +254,9 @@ impl PhysicsPipeline {
                     bodies,
                     &mut manifolds[..],
                     &self.manifold_indices[island_id],
-                    joints.joints_mut(),
+                    impulse_joints.joints_mut(),
                     &self.joint_constraint_indices[island_id],
+                    multibody_joints,
                 )
             }
         }
@@ -311,7 +271,9 @@ impl PhysicsPipeline {
             let solvers = &mut self.solvers[..num_islands];
             let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _);
             let manifolds = &std::sync::atomic::AtomicPtr::new(&mut manifolds as *mut _);
-            let joints = &std::sync::atomic::AtomicPtr::new(joints.joints_vec_mut() as *mut _);
+            let impulse_joints =
+                &std::sync::atomic::AtomicPtr::new(impulse_joints.joints_vec_mut() as *mut _);
+            let multibody_joints = &std::sync::atomic::AtomicPtr::new(multibody_joints as *mut _);
             let manifold_indices = &self.manifold_indices[..];
             let joint_constraint_indices = &self.joint_constraint_indices[..];
 
@@ -326,10 +288,13 @@ impl PhysicsPipeline {
                             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 impulse_joints: &mut Vec<JointGraphEdge> =
+                            unsafe { std::mem::transmute(impulse_joints.load(Ordering::Relaxed)) };
+                        let multibody_joints: &mut MultibodyJointSet = unsafe {
+                            std::mem::transmute(multibody_joints.load(Ordering::Relaxed))
+                        };
 
-                        solver.init_constraints_and_solve_velocity_constraints(
+                        solver.init_and_solve(
                             scope,
                             island_id,
                             islands,
@@ -337,8 +302,9 @@ impl PhysicsPipeline {
                             bodies,
                             manifolds,
                             &manifold_indices[island_id],
-                            joints,
+                            impulse_joints,
                             &joint_constraint_indices[island_id],
+                            multibody_joints,
                         )
                     });
             });
@@ -485,7 +451,8 @@ impl PhysicsPipeline {
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
-        joints: &mut JointSet,
+        impulse_joints: &mut ImpulseJointSet,
+        multibody_joints: &mut MultibodyJointSet,
         ccd_solver: &mut CCDSolver,
         hooks: &dyn PhysicsHooks<RigidBodySet, ColliderSet>,
         events: &dyn EventHandler,
@@ -505,7 +472,8 @@ impl PhysicsPipeline {
             &mut modified_bodies,
             &mut modified_colliders,
             &mut removed_colliders,
-            joints,
+            impulse_joints,
+            multibody_joints,
             ccd_solver,
             hooks,
             events,
@@ -525,7 +493,8 @@ impl PhysicsPipeline {
         modified_bodies: &mut Vec<RigidBodyHandle>,
         modified_colliders: &mut Vec<ColliderHandle>,
         removed_colliders: &mut Vec<ColliderHandle>,
-        joints: &mut JointSet,
+        impulse_joints: &mut ImpulseJointSet,
+        multibody_joints: &mut MultibodyJointSet,
         ccd_solver: &mut CCDSolver,
         hooks: &dyn PhysicsHooks<Bodies, Colliders>,
         events: &dyn EventHandler,
@@ -567,6 +536,15 @@ impl PhysicsPipeline {
             modified_colliders,
         );
 
+        // TODO: do this only on user-change.
+        // TODO: do we want some kind of automatic inverse kinematics?
+        for multibody in &mut multibody_joints.multibodies {
+            multibody.1.update_root_type(bodies);
+            // FIXME: what should we do here? We should not
+            //        rely on the next state here.
+            multibody.1.forward_kinematics_next(bodies, true);
+        }
+
         self.detect_collisions(
             integration_parameters,
             islands,
@@ -662,7 +640,8 @@ impl PhysicsPipeline {
                 narrow_phase,
                 bodies,
                 colliders,
-                joints,
+                impulse_joints,
+                multibody_joints,
             );
 
             // If CCD is enabled, execute the CCD motion clamping.
@@ -688,15 +667,6 @@ impl PhysicsPipeline {
                 }
             }
 
-            // NOTE: we need to run the position solver **after** the
-            //       CCD motion clamping because otherwise the clamping
-            //       would undo the depenetration done by the position
-            //       solver.
-            //       This happens because our CCD use the real rigid-body
-            //       velocities instead of just interpolating between
-            //       isometries.
-            self.solve_position_constraints(&integration_parameters, islands, bodies);
-
             let clear_forces = remaining_substeps == 0;
             self.advance_to_final_positions(
                 islands,
@@ -705,6 +675,7 @@ impl PhysicsPipeline {
                 modified_colliders,
                 clear_forces,
             );
+
             self.detect_collisions(
                 &integration_parameters,
                 islands,
@@ -729,7 +700,8 @@ impl PhysicsPipeline {
 #[cfg(test)]
 mod test {
     use crate::dynamics::{
-        CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodyBuilder, RigidBodySet,
+        CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, RigidBodyBuilder,
+        RigidBodySet,
     };
     use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase};
     use crate::math::Vector;
@@ -738,7 +710,7 @@ mod test {
     #[test]
     fn kinematic_and_static_contact_crash() {
         let mut colliders = ColliderSet::new();
-        let mut joints = JointSet::new();
+        let mut impulse_joints = ImpulseJointSet::new();
         let mut pipeline = PhysicsPipeline::new();
         let mut bf = BroadPhase::new();
         let mut nf = NarrowPhase::new();
@@ -763,7 +735,7 @@ mod test {
             &mut nf,
             &mut bodies,
             &mut colliders,
-            &mut joints,
+            &mut impulse_joints,
             &mut CCDSolver::new(),
             &(),
             &(),
@@ -773,7 +745,7 @@ mod test {
     #[test]
     fn rigid_body_removal_before_step() {
         let mut colliders = ColliderSet::new();
-        let mut joints = JointSet::new();
+        let mut impulse_joints = ImpulseJointSet::new();
         let mut pipeline = PhysicsPipeline::new();
         let mut bf = BroadPhase::new();
         let mut nf = NarrowPhase::new();
@@ -798,7 +770,7 @@ mod test {
 
         let to_delete = [h1, h2, h3, h4];
         for h in &to_delete {
-            bodies.remove(*h, &mut islands, &mut colliders, &mut joints);
+            bodies.remove(*h, &mut islands, &mut colliders, &mut impulse_joints);
         }
 
         pipeline.step(
@@ -809,7 +781,7 @@ mod test {
             &mut nf,
             &mut bodies,
             &mut colliders,
-            &mut joints,
+            &mut impulse_joints,
             &mut CCDSolver::new(),
             &(),
             &(),
@@ -820,7 +792,7 @@ mod test {
     #[test]
     fn rigid_body_removal_snapshot_handle_determinism() {
         let mut colliders = ColliderSet::new();
-        let mut joints = JointSet::new();
+        let mut impulse_joints = ImpulseJointSet::new();
         let mut islands = IslandManager::new();
 
         let mut bodies = RigidBodySet::new();
@@ -829,9 +801,9 @@ mod test {
         let h2 = bodies.insert(rb.clone());
         let h3 = bodies.insert(rb.clone());
 
-        bodies.remove(h1, &mut islands, &mut colliders, &mut joints);
-        bodies.remove(h3, &mut islands, &mut colliders, &mut joints);
-        bodies.remove(h2, &mut islands, &mut colliders, &mut joints);
+        bodies.remove(h1, &mut islands, &mut colliders, &mut impulse_joints);
+        bodies.remove(h3, &mut islands, &mut colliders, &mut impulse_joints);
+        bodies.remove(h2, &mut islands, &mut colliders, &mut impulse_joints);
 
         let ser_bodies = bincode::serialize(&bodies).unwrap();
         let mut bodies2: RigidBodySet = bincode::deserialize(&ser_bodies).unwrap();
@@ -859,7 +831,7 @@ mod test {
         let mut bodies = RigidBodySet::new();
         let mut colliders = ColliderSet::new();
         let mut ccd = CCDSolver::new();
-        let mut joints = JointSet::new();
+        let mut impulse_joints = ImpulseJointSet::new();
         let mut islands = IslandManager::new();
         let physics_hooks = ();
         let event_handler = ();
@@ -869,7 +841,7 @@ mod test {
         let collider = ColliderBuilder::ball(1.0).build();
         let c_handle = colliders.insert_with_parent(collider, b_handle, &mut bodies);
         colliders.remove(c_handle, &mut islands, &mut bodies, true);
-        bodies.remove(b_handle, &mut islands, &mut colliders, &mut joints);
+        bodies.remove(b_handle, &mut islands, &mut colliders, &mut impulse_joints);
 
         for _ in 0..10 {
             pipeline.step(
@@ -880,7 +852,7 @@ mod test {
                 &mut narrow_phase,
                 &mut bodies,
                 &mut colliders,
-                &mut joints,
+                &mut impulse_joints,
                 &mut ccd,
                 &physics_hooks,
                 &event_handler,
diff --git a/src/utils.rs b/src/utils.rs
index 7f0cf46..9aa4bf2 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -1,6 +1,9 @@
 //! Miscellaneous utilities.
 
-use na::{Matrix3, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3};
+use na::{
+    Matrix1, Matrix2, Matrix3, Point2, Point3, RowVector2, Scalar, SimdRealField, UnitComplex,
+    UnitQuaternion, Vector1, Vector2, Vector3,
+};
 use num::Zero;
 use simba::simd::SimdValue;
 use std::ops::IndexMut;
@@ -12,6 +15,10 @@ use {
     num::One,
 };
 
+pub trait WReal: SimdRealField<Element = Real> + Copy {}
+impl WReal for Real {}
+impl WReal for SimdReal {}
+
 pub(crate) fn inv(val: Real) -> Real {
     if val == 0.0 {
         0.0
@@ -20,6 +27,10 @@ pub(crate) fn inv(val: Real) -> Real {
     }
 }
 
+pub(crate) fn simd_inv<N: SimdRealField + Copy>(val: N) -> N {
+    N::zero().select(val.simd_eq(N::zero()), N::one() / val)
+}
+
 /// Trait to copy the sign of each component of one scalar/vector/matrix to another.
 pub trait WSign<Rhs>: Sized {
     // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652
@@ -234,32 +245,79 @@ where
 
 pub(crate) trait WCrossMatrix: Sized {
     type CrossMat;
+    type CrossMatTr;
 
     fn gcross_matrix(self) -> Self::CrossMat;
+    fn gcross_matrix_tr(self) -> Self::CrossMatTr;
 }
 
-impl WCrossMatrix for Vector3<Real> {
-    type CrossMat = Matrix3<Real>;
+impl<N: SimdRealField + Copy> WCrossMatrix for Vector3<N> {
+    type CrossMat = Matrix3<N>;
+    type CrossMatTr = Matrix3<N>;
 
     #[inline]
     #[rustfmt::skip]
     fn gcross_matrix(self) -> Self::CrossMat {
         Matrix3::new(
-            0.0, -self.z, self.y,
-            self.z, 0.0, -self.x,
-            -self.y, self.x, 0.0,
+            N::zero(), -self.z, self.y,
+            self.z, N::zero(), -self.x,
+            -self.y, self.x, N::zero(),
+        )
+    }
+
+    #[inline]
+    #[rustfmt::skip]
+    fn gcross_matrix_tr(self) -> Self::CrossMatTr {
+        Matrix3::new(
+            N::zero(), self.z, -self.y,
+            -self.z, N::zero(), self.x,
+            self.y, -self.x, N::zero(),
         )
     }
 }
 
-impl WCrossMatrix for Vector2<Real> {
-    type CrossMat = Vector2<Real>;
+impl<N: SimdRealField + Copy> WCrossMatrix for Vector2<N> {
+    type CrossMat = RowVector2<N>;
+    type CrossMatTr = Vector2<N>;
 
     #[inline]
     fn gcross_matrix(self) -> Self::CrossMat {
+        RowVector2::new(-self.y, self.x)
+    }
+    #[inline]
+    fn gcross_matrix_tr(self) -> Self::CrossMatTr {
         Vector2::new(-self.y, self.x)
     }
 }
+impl WCrossMatrix for Real {
+    type CrossMat = Matrix2<Real>;
+    type CrossMatTr = Matrix2<Real>;
+
+    #[inline]
+    fn gcross_matrix(self) -> Matrix2<Real> {
+        Matrix2::new(0.0, -self, self, 0.0)
+    }
+
+    #[inline]
+    fn gcross_matrix_tr(self) -> Matrix2<Real> {
+        Matrix2::new(0.0, self, -self, 0.0)
+    }
+}
+
+impl WCrossMatrix for SimdReal {
+    type CrossMat = Matrix2<SimdReal>;
+    type CrossMatTr = Matrix2<SimdReal>;
+
+    #[inline]
+    fn gcross_matrix(self) -> Matrix2<SimdReal> {
+        Matrix2::new(SimdReal::zero(), -self, self, SimdReal::zero())
+    }
+
+    #[inline]
+    fn gcross_matrix_tr(self) -> Matrix2<SimdReal> {
+        Matrix2::new(SimdReal::zero(), self, -self, SimdReal::zero())
+    }
+}
 
 pub(crate) trait WCross<Rhs>: Sized {
     type Result;
@@ -295,50 +353,43 @@ pub(crate) trait WDot<Rhs>: Sized {
     fn gdot(&self, rhs: Rhs) -> Self::Result;
 }
 
-impl WDot<Vector3<Real>> for Vector3<Real> {
-    type Result = Real;
+impl<N: SimdRealField + Copy> WDot<Vector3<N>> for Vector3<N> {
+    type Result = N;
 
-    fn gdot(&self, rhs: Vector3<Real>) -> Self::Result {
+    fn gdot(&self, rhs: Vector3<N>) -> Self::Result {
         self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
     }
 }
 
-impl WDot<Vector2<Real>> for Vector2<Real> {
-    type Result = Real;
+impl<N: SimdRealField + Copy> WDot<Vector2<N>> for Vector2<N> {
+    type Result = N;
 
-    fn gdot(&self, rhs: Vector2<Real>) -> Self::Result {
+    fn gdot(&self, rhs: Vector2<N>) -> Self::Result {
         self.x * rhs.x + self.y * rhs.y
     }
 }
 
-impl WDot<Real> for Real {
-    type Result = Real;
+impl<N: SimdRealField + Copy> WDot<Vector1<N>> for N {
+    type Result = N;
 
-    fn gdot(&self, rhs: Real) -> Self::Result {
+    fn gdot(&self, rhs: Vector1<N>) -> Self::Result {
+        *self * rhs.x
+    }
+}
+
+impl<N: WReal> WDot<N> for N {
+    type Result = N;
+
+    fn gdot(&self, rhs: N) -> Self::Result {
         *self * rhs
     }
 }
 
-impl WCrossMatrix for Vector3<SimdReal> {
-    type CrossMat = Matrix3<SimdReal>;
+impl<N: SimdRealField + Copy> WDot<N> for Vector1<N> {
+    type Result = N;
 
-    #[inline]
-    #[rustfmt::skip]
-    fn gcross_matrix(self) -> Self::CrossMat {
-        Matrix3::new(
-            SimdReal::zero(), -self.z, self.y,
-            self.z, SimdReal::zero(), -self.x,
-            -self.y, self.x, SimdReal::zero(),
-        )
-    }
-}
-
-impl WCrossMatrix for Vector2<SimdReal> {
-    type CrossMat = Vector2<SimdReal>;
-
-    #[inline]
-    fn gcross_matrix(self) -> Self::CrossMat {
-        Vector2::new(-self.y, self.x)
+    fn gdot(&self, rhs: N) -> Self::Result {
+        self.x * rhs
     }
 }
 
@@ -368,27 +419,42 @@ impl WCross<Vector2<SimdReal>> for Vector2<SimdReal> {
     }
 }
 
-impl WDot<Vector3<SimdReal>> for Vector3<SimdReal> {
-    type Result = SimdReal;
+pub trait WQuat<N> {
+    type Result;
 
-    fn gdot(&self, rhs: Vector3<SimdReal>) -> Self::Result {
-        self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
+    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result;
+}
+
+impl<N: SimdRealField + Copy> WQuat<N> for UnitComplex<N>
+where
+    <N as SimdValue>::Element: SimdRealField,
+{
+    type Result = Matrix1<N>;
+
+    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
+        let two: N = na::convert(2.0);
+        Matrix1::new((self.im * rhs.im + self.re * rhs.re) * two)
     }
 }
 
-impl WDot<Vector2<SimdReal>> for Vector2<SimdReal> {
-    type Result = SimdReal;
+impl<N: SimdRealField + Copy> WQuat<N> for UnitQuaternion<N>
+where
+    <N as SimdValue>::Element: SimdRealField,
+{
+    type Result = Matrix3<N>;
 
-    fn gdot(&self, rhs: Vector2<SimdReal>) -> Self::Result {
-        self.x * rhs.x + self.y * rhs.y
-    }
-}
+    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
+        let half: N = na::convert(0.5);
+        let v1 = self.imag();
+        let v2 = rhs.imag();
+        let w1 = self.w;
+        let w2 = rhs.w;
 
-impl WDot<SimdReal> for SimdReal {
-    type Result = SimdReal;
-
-    fn gdot(&self, rhs: SimdReal) -> Self::Result {
-        *self * rhs
+        // TODO: this can probably be optimized a lot by unrolling the ops.
+        (v1 * v2.transpose() + Matrix3::from_diagonal_element(w1 * w2)
+            - (v1 * w2 + v2 * w1).cross_matrix()
+            + v1.cross_matrix() * v2.cross_matrix())
+            * half
     }
 }
 
@@ -404,59 +470,23 @@ pub(crate) trait WAngularInertia<N> {
     fn into_matrix(self) -> Self::AngMatrix;
 }
 
-impl WAngularInertia<Real> for Real {
-    type AngVector = Real;
-    type LinVector = Vector2<Real>;
-    type AngMatrix = Real;
+impl<N: WReal> WAngularInertia<N> for N {
+    type AngVector = N;
+    type LinVector = Vector2<N>;
+    type AngMatrix = N;
 
     fn inverse(&self) -> Self {
-        if *self != 0.0 {
-            1.0 / *self
-        } else {
-            0.0
-        }
+        simd_inv(*self)
     }
 
-    fn transform_lin_vector(&self, pt: Vector2<Real>) -> Vector2<Real> {
-        *self * pt
+    fn transform_lin_vector(&self, pt: Vector2<N>) -> Vector2<N> {
+        pt * *self
     }
-    fn transform_vector(&self, pt: Real) -> Real {
-        *self * pt
-    }
-
-    fn squared(&self) -> Real {
-        *self * *self
-    }
-
-    fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix {
-        mat * *self
-    }
-
-    fn into_matrix(self) -> Self::AngMatrix {
-        self
-    }
-}
-
-impl WAngularInertia<SimdReal> for SimdReal {
-    type AngVector = SimdReal;
-    type LinVector = Vector2<SimdReal>;
-    type AngMatrix = SimdReal;
-
-    fn inverse(&self) -> Self {
-        let zero = <SimdReal>::zero();
-        let is_zero = self.simd_eq(zero);
-        (<SimdReal>::one() / *self).select(is_zero, zero)
-    }
-
-    fn transform_lin_vector(&self, pt: Vector2<SimdReal>) -> Vector2<SimdReal> {
+    fn transform_vector(&self, pt: N) -> N {
         pt * *self
     }
 
-    fn transform_vector(&self, pt: SimdReal) -> SimdReal {
-        *self * pt
-    }
-
-    fn squared(&self) -> SimdReal {
+    fn squared(&self) -> N {
         *self * *self
     }
 
@@ -757,3 +787,17 @@ impl<T> IndexMut2<usize> for Vec<T> {
         }
     }
 }
+
+impl<T> IndexMut2<usize> for [T] {
+    #[inline]
+    fn index_mut2(&mut self, i: usize, j: usize) -> (&mut T, &mut T) {
+        assert!(i != j, "Unable to index the same element twice.");
+        assert!(i < self.len() && j < self.len(), "Index out of bounds.");
+
+        unsafe {
+            let a = &mut *(self.get_unchecked_mut(i) as *mut _);
+            let b = &mut *(self.get_unchecked_mut(j) as *mut _);
+            (a, b)
+        }
+    }
+}
diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs
index f0bffa3..2a143fb 100644
--- a/src_testbed/box2d_backend.rs
+++ b/src_testbed/box2d_backend.rs
@@ -2,14 +2,12 @@ use std::collections::HashMap;
 
 use na::{Isometry2, Vector2};
 use rapier::counters::Counters;
-use rapier::dynamics::{
-    IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
-};
+use rapier::dynamics::{ImpulseJointSet, IntegrationParameters, RigidBodyHandle, RigidBodySet};
 use rapier::geometry::{Collider, ColliderSet};
 use std::f32;
 
 use wrapped2d::b2;
-use wrapped2d::dynamics::joints::{PrismaticJointDef, RevoluteJointDef, WeldJointDef};
+// use wrapped2d::dynamics::joints::{PrismaticJointDef, RevoluteJointDef, WeldJointDef};
 use wrapped2d::user_data::NoUserData;
 
 fn na_vec_to_b2_vec(v: Vector2<f32>) -> b2::Vec2 {
@@ -34,7 +32,7 @@ impl Box2dWorld {
         gravity: Vector2<f32>,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
-        joints: &JointSet,
+        impulse_joints: &ImpulseJointSet,
     ) -> Self {
         let mut world = b2::World::new(&na_vec_to_b2_vec(gravity));
         world.set_continuous_physics(bodies.iter().any(|b| b.1.is_ccd_enabled()));
@@ -46,7 +44,7 @@ impl Box2dWorld {
 
         res.insert_bodies(bodies);
         res.insert_colliders(colliders);
-        res.insert_joints(joints);
+        res.insert_joints(impulse_joints);
         res
     }
 
@@ -95,8 +93,9 @@ impl Box2dWorld {
         }
     }
 
-    fn insert_joints(&mut self, joints: &JointSet) {
-        for joint in joints.iter() {
+    fn insert_joints(&mut self, _impulse_joints: &ImpulseJointSet) {
+        /*
+        for joint in impulse_joints.iter() {
             let body_a = self.rapier2box2d[&joint.1.body1];
             let body_b = self.rapier2box2d[&joint.1.body2];
 
@@ -154,6 +153,8 @@ impl Box2dWorld {
                 }
             }
         }
+
+         */
     }
 
     fn create_fixture(collider: &Collider, body: &mut b2::MetaBody<NoUserData>) {
@@ -225,7 +226,7 @@ impl Box2dWorld {
         self.world.step(
             params.dt,
             params.max_velocity_iterations as i32,
-            params.max_position_iterations as i32,
+            params.max_stabilization_iterations as i32,
         );
         counters.step_completed();
     }
diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs
index 0bc08d0..3358a70 100644
--- a/src_testbed/harness/mod.rs
+++ b/src_testbed/harness/mod.rs
@@ -3,7 +3,10 @@ use crate::{
     TestbedGraphics,
 };
 use plugin::HarnessPlugin;
-use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet};
+use rapier::dynamics::{
+    CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
+    RigidBodySet,
+};
 use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
 use rapier::math::Vector;
 use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
@@ -78,9 +81,14 @@ impl Harness {
         }
     }
 
-    pub fn new(bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) -> Self {
+    pub fn new(
+        bodies: RigidBodySet,
+        colliders: ColliderSet,
+        impulse_joints: ImpulseJointSet,
+        multibody_joints: MultibodyJointSet,
+    ) -> Self {
         let mut res = Self::new_empty();
-        res.set_world(bodies, colliders, joints);
+        res.set_world(bodies, colliders, impulse_joints, multibody_joints);
         res
     }
 
@@ -100,24 +108,39 @@ impl Harness {
         &mut self.physics
     }
 
-    pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) {
-        self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ())
+    pub fn set_world(
+        &mut self,
+        bodies: RigidBodySet,
+        colliders: ColliderSet,
+        impulse_joints: ImpulseJointSet,
+        multibody_joints: MultibodyJointSet,
+    ) {
+        self.set_world_with_params(
+            bodies,
+            colliders,
+            impulse_joints,
+            multibody_joints,
+            Vector::y() * -9.81,
+            (),
+        )
     }
 
     pub fn set_world_with_params(
         &mut self,
         bodies: RigidBodySet,
         colliders: ColliderSet,
-        joints: JointSet,
+        impulse_joints: ImpulseJointSet,
+        multibody_joints: MultibodyJointSet,
         gravity: Vector<f32>,
         hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
     ) {
         // println!("Num bodies: {}", bodies.len());
-        // println!("Num joints: {}", joints.len());
+        // println!("Num impulse_joints: {}", impulse_joints.len());
         self.physics.gravity = gravity;
         self.physics.bodies = bodies;
         self.physics.colliders = colliders;
-        self.physics.joints = joints;
+        self.physics.impulse_joints = impulse_joints;
+        self.physics.multibody_joints = multibody_joints;
         self.physics.hooks = Box::new(hooks);
 
         self.physics.islands = IslandManager::new();
@@ -162,7 +185,8 @@ impl Harness {
                     &mut physics.narrow_phase,
                     &mut physics.bodies,
                     &mut physics.colliders,
-                    &mut physics.joints,
+                    &mut physics.impulse_joints,
+                    &mut physics.multibody_joints,
                     &mut physics.ccd_solver,
                     &*physics.hooks,
                     event_handler,
@@ -179,7 +203,8 @@ impl Harness {
             &mut self.physics.narrow_phase,
             &mut self.physics.bodies,
             &mut self.physics.colliders,
-            &mut self.physics.joints,
+            &mut self.physics.impulse_joints,
+            &mut self.physics.multibody_joints,
             &mut self.physics.ccd_solver,
             &*self.physics.hooks,
             &self.event_handler,
diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs
index a3e3a00..b9a8822 100644
--- a/src_testbed/lib.rs
+++ b/src_testbed/lib.rs
@@ -1,12 +1,4 @@
 extern crate nalgebra as na;
-#[cfg(all(feature = "dim2", feature = "other-backends"))]
-extern crate ncollide2d as ncollide;
-#[cfg(all(feature = "dim3", feature = "other-backends"))]
-extern crate ncollide3d as ncollide;
-#[cfg(all(feature = "dim2", feature = "other-backends"))]
-extern crate nphysics2d as nphysics;
-#[cfg(all(feature = "dim3", feature = "other-backends"))]
-extern crate nphysics3d as nphysics;
 #[cfg(feature = "dim2")]
 extern crate parry2d as parry;
 #[cfg(feature = "dim3")]
@@ -37,8 +29,6 @@ mod camera2d;
 mod camera3d;
 mod graphics;
 pub mod harness;
-#[cfg(feature = "other-backends")]
-mod nphysics_backend;
 pub mod objects;
 pub mod physics;
 #[cfg(all(feature = "dim3", feature = "other-backends"))]
diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
deleted file mode 100644
index c7d6c0e..0000000
--- a/src_testbed/nphysics_backend.rs
+++ /dev/null
@@ -1,248 +0,0 @@
-#[cfg(feature = "dim2")]
-use ncollide::shape::ConvexPolygon;
-use ncollide::shape::{Ball, Capsule, Cuboid, HeightField, ShapeHandle};
-use nphysics::force_generator::DefaultForceGeneratorSet;
-use nphysics::joint::{
-    DefaultJointConstraintSet, FixedConstraint, PrismaticConstraint, RevoluteConstraint,
-};
-use nphysics::object::{
-    BodyPartHandle, ColliderDesc, DefaultBodyHandle, DefaultBodySet, DefaultColliderSet,
-    RigidBodyDesc,
-};
-use nphysics::world::{DefaultGeometricalWorld, DefaultMechanicalWorld};
-use rapier::counters::Counters;
-use rapier::dynamics::{
-    IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
-};
-use rapier::geometry::{Collider, ColliderSet};
-use rapier::math::Vector;
-use std::collections::HashMap;
-#[cfg(feature = "dim3")]
-use {ncollide::shape::TriMesh, nphysics::joint::BallConstraint};
-
-pub struct NPhysicsWorld {
-    rapier2nphysics: HashMap<RigidBodyHandle, DefaultBodyHandle>,
-    mechanical_world: DefaultMechanicalWorld<f32>,
-    geometrical_world: DefaultGeometricalWorld<f32>,
-    bodies: DefaultBodySet<f32>,
-    colliders: DefaultColliderSet<f32>,
-    joints: DefaultJointConstraintSet<f32>,
-    force_generators: DefaultForceGeneratorSet<f32>,
-}
-
-impl NPhysicsWorld {
-    pub fn from_rapier(
-        gravity: Vector<f32>,
-        bodies: &RigidBodySet,
-        colliders: &ColliderSet,
-        joints: &JointSet,
-    ) -> Self {
-        let mut rapier2nphysics = HashMap::new();
-
-        let mechanical_world = DefaultMechanicalWorld::new(gravity);
-        let geometrical_world = DefaultGeometricalWorld::new();
-        let mut nphysics_bodies = DefaultBodySet::new();
-        let mut nphysics_colliders = DefaultColliderSet::new();
-        let mut nphysics_joints = DefaultJointConstraintSet::new();
-        let force_generators = DefaultForceGeneratorSet::new();
-
-        for (rapier_handle, rb) in bodies.iter() {
-            // let material = physics.create_material(rb.collider.friction, rb.collider.friction, 0.0);
-            let nphysics_rb = RigidBodyDesc::new().position(*rb.position()).build();
-            let nphysics_rb_handle = nphysics_bodies.insert(nphysics_rb);
-
-            rapier2nphysics.insert(rapier_handle, nphysics_rb_handle);
-        }
-
-        for (_, collider) in colliders.iter() {
-            if let Some(parent_handle) = collider.parent() {
-                let parent = &bodies[parent_handle];
-                let nphysics_rb_handle = rapier2nphysics[&parent_handle];
-                if let Some(collider) =
-                    nphysics_collider_from_rapier_collider(&collider, parent.is_dynamic())
-                {
-                    let nphysics_collider = collider.build(BodyPartHandle(nphysics_rb_handle, 0));
-                    nphysics_colliders.insert(nphysics_collider);
-                } else {
-                    eprintln!("Creating shape unknown to the nphysics backend.")
-                }
-            }
-        }
-
-        for joint in joints.iter() {
-            let b1 = BodyPartHandle(rapier2nphysics[&joint.1.body1], 0);
-            let b2 = BodyPartHandle(rapier2nphysics[&joint.1.body2], 0);
-
-            match &joint.1.params {
-                JointParams::FixedJoint(params) => {
-                    let c = FixedConstraint::new(
-                        b1,
-                        b2,
-                        params.local_frame1.translation.vector.into(),
-                        params.local_frame1.rotation,
-                        params.local_frame2.translation.vector.into(),
-                        params.local_frame2.rotation,
-                    );
-                    nphysics_joints.insert(c);
-                }
-                #[cfg(feature = "dim3")]
-                JointParams::BallJoint(params) => {
-                    let c = BallConstraint::new(b1, b2, params.local_anchor1, params.local_anchor2);
-                    nphysics_joints.insert(c);
-                }
-                #[cfg(feature = "dim2")]
-                JointParams::BallJoint(params) => {
-                    let c =
-                        RevoluteConstraint::new(b1, b2, params.local_anchor1, params.local_anchor2);
-                    nphysics_joints.insert(c);
-                }
-                #[cfg(feature = "dim3")]
-                JointParams::RevoluteJoint(params) => {
-                    let c = RevoluteConstraint::new(
-                        b1,
-                        b2,
-                        params.local_anchor1,
-                        params.local_axis1,
-                        params.local_anchor2,
-                        params.local_axis2,
-                    );
-                    nphysics_joints.insert(c);
-                }
-                JointParams::PrismaticJoint(params) => {
-                    let mut c = PrismaticConstraint::new(
-                        b1,
-                        b2,
-                        params.local_anchor1,
-                        params.local_axis1(),
-                        params.local_anchor2,
-                    );
-
-                    if params.limits_enabled {
-                        c.enable_min_offset(params.limits[0]);
-                        c.enable_max_offset(params.limits[1]);
-                    }
-
-                    nphysics_joints.insert(c);
-                } // JointParams::GenericJoint(_) => {
-                  //     eprintln!(
-                  //         "Joint type currently unsupported by the nphysics backend: GenericJoint."
-                  //     )
-                  // }
-            }
-        }
-
-        Self {
-            rapier2nphysics,
-            mechanical_world,
-            geometrical_world,
-            bodies: nphysics_bodies,
-            colliders: nphysics_colliders,
-            joints: nphysics_joints,
-            force_generators,
-        }
-    }
-
-    pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) {
-        self.mechanical_world
-            .integration_parameters
-            .max_position_iterations = params.max_position_iterations;
-        self.mechanical_world
-            .integration_parameters
-            .max_velocity_iterations = params.max_velocity_iterations;
-        self.mechanical_world
-            .integration_parameters
-            .set_dt(params.dt);
-        self.mechanical_world.integration_parameters.warmstart_coeff = params.warmstart_coeff;
-
-        counters.step_started();
-        self.mechanical_world.step(
-            &mut self.geometrical_world,
-            &mut self.bodies,
-            &mut self.colliders,
-            &mut self.joints,
-            &mut self.force_generators,
-        );
-        counters.step_completed();
-    }
-
-    pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) {
-        for (rapier_handle, nphysics_handle) in self.rapier2nphysics.iter() {
-            let rb = bodies.get_mut(*rapier_handle).unwrap();
-            let ra = self.bodies.rigid_body(*nphysics_handle).unwrap();
-            let pos = *ra.position();
-            rb.set_position(pos, false);
-
-            for coll_handle in rb.colliders() {
-                let collider = &mut colliders[*coll_handle];
-                collider.set_position(
-                    pos * collider.position_wrt_parent().copied().unwrap_or(na::one()),
-                );
-            }
-        }
-    }
-}
-
-fn nphysics_collider_from_rapier_collider(
-    collider: &Collider,
-    is_dynamic: bool,
-) -> Option<ColliderDesc<f32>> {
-    let mut margin = ColliderDesc::<f32>::default_margin();
-    let mut pos = collider.position_wrt_parent().copied().unwrap_or(na::one());
-    let shape = collider.shape();
-
-    let shape = if let Some(cuboid) = shape.as_cuboid() {
-        ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin)))
-    } else if let Some(cuboid) = shape.as_round_cuboid() {
-        margin = cuboid.border_radius;
-        ShapeHandle::new(Cuboid::new(cuboid.base_shape.half_extents))
-    } else if let Some(ball) = shape.as_ball() {
-        ShapeHandle::new(Ball::new(ball.radius - margin))
-    } else if let Some(capsule) = shape.as_capsule() {
-        pos *= capsule.transform_wrt_y();
-        ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
-    } else if let Some(heightfield) = shape.as_heightfield() {
-        let heights = heightfield.heights();
-        let scale = heightfield.scale();
-        let heightfield = HeightField::new(heights.clone(), *scale);
-        ShapeHandle::new(heightfield)
-    } else {
-        #[cfg(feature = "dim3")]
-        if let Some(trimesh) = shape.as_trimesh() {
-            ShapeHandle::new(TriMesh::new(
-                trimesh.vertices().to_vec(),
-                trimesh
-                    .indices()
-                    .iter()
-                    .map(|idx| na::point![idx[0] as usize, idx[1] as usize, idx[2] as usize])
-                    .collect(),
-                None,
-            ))
-        } else {
-            return None;
-        }
-
-        #[cfg(feature = "dim2")]
-        if let Some(polygon) = shape.as_round_convex_polygon() {
-            margin = polygon.border_radius;
-            ShapeHandle::new(ConvexPolygon::try_from_points(polygon.base_shape.points()).unwrap())
-        } else if let Some(polygon) = shape.as_convex_polygon() {
-            ShapeHandle::new(ConvexPolygon::try_from_points(polygon.points()).unwrap())
-        } else {
-            return None;
-        }
-    };
-
-    let density = if is_dynamic {
-        collider.density().unwrap_or(0.0)
-    } else {
-        0.0
-    };
-
-    Some(
-        ColliderDesc::new(shape)
-            .position(pos)
-            .density(density)
-            .sensor(collider.is_sensor())
-            .margin(margin),
-    )
-}
diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs
index 29e9a46..13bf915 100644
--- a/src_testbed/physics/mod.rs
+++ b/src_testbed/physics/mod.rs
@@ -1,5 +1,8 @@
 use crossbeam::channel::Receiver;
-use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet};
+use rapier::dynamics::{
+    CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
+    RigidBodySet,
+};
 use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase};
 use rapier::math::Vector;
 use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
@@ -10,7 +13,7 @@ pub struct PhysicsSnapshot {
     narrow_phase: Vec<u8>,
     bodies: Vec<u8>,
     colliders: Vec<u8>,
-    joints: Vec<u8>,
+    impulse_joints: Vec<u8>,
 }
 
 impl PhysicsSnapshot {
@@ -20,7 +23,7 @@ impl PhysicsSnapshot {
         narrow_phase: &NarrowPhase,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
-        joints: &JointSet,
+        impulse_joints: &ImpulseJointSet,
     ) -> bincode::Result<Self> {
         Ok(Self {
             timestep_id,
@@ -28,7 +31,7 @@ impl PhysicsSnapshot {
             narrow_phase: bincode::serialize(narrow_phase)?,
             bodies: bincode::serialize(bodies)?,
             colliders: bincode::serialize(colliders)?,
-            joints: bincode::serialize(joints)?,
+            impulse_joints: bincode::serialize(impulse_joints)?,
         })
     }
 
@@ -40,7 +43,7 @@ impl PhysicsSnapshot {
         NarrowPhase,
         RigidBodySet,
         ColliderSet,
-        JointSet,
+        ImpulseJointSet,
     )> {
         Ok((
             self.timestep_id,
@@ -48,7 +51,7 @@ impl PhysicsSnapshot {
             bincode::deserialize(&self.narrow_phase)?,
             bincode::deserialize(&self.bodies)?,
             bincode::deserialize(&self.colliders)?,
-            bincode::deserialize(&self.joints)?,
+            bincode::deserialize(&self.impulse_joints)?,
         ))
     }
 
@@ -57,13 +60,13 @@ impl PhysicsSnapshot {
             + self.narrow_phase.len()
             + self.bodies.len()
             + self.colliders.len()
-            + self.joints.len();
+            + self.impulse_joints.len();
         println!("Snapshot length: {}B", total);
         println!("|_ broad_phase: {}B", self.broad_phase.len());
         println!("|_ narrow_phase: {}B", self.narrow_phase.len());
         println!("|_ bodies: {}B", self.bodies.len());
         println!("|_ colliders: {}B", self.colliders.len());
-        println!("|_ joints: {}B", self.joints.len());
+        println!("|_ impulse_joints: {}B", self.impulse_joints.len());
     }
 }
 
@@ -73,7 +76,8 @@ pub struct PhysicsState {
     pub narrow_phase: NarrowPhase,
     pub bodies: RigidBodySet,
     pub colliders: ColliderSet,
-    pub joints: JointSet,
+    pub impulse_joints: ImpulseJointSet,
+    pub multibody_joints: MultibodyJointSet,
     pub ccd_solver: CCDSolver,
     pub pipeline: PhysicsPipeline,
     pub query_pipeline: QueryPipeline,
@@ -90,7 +94,8 @@ impl PhysicsState {
             narrow_phase: NarrowPhase::new(),
             bodies: RigidBodySet::new(),
             colliders: ColliderSet::new(),
-            joints: JointSet::new(),
+            impulse_joints: ImpulseJointSet::new(),
+            multibody_joints: MultibodyJointSet::new(),
             ccd_solver: CCDSolver::new(),
             pipeline: PhysicsPipeline::new(),
             query_pipeline: QueryPipeline::new(),
diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs
index 6c3155f..9eec3ff 100644
--- a/src_testbed/physx_backend.rs
+++ b/src_testbed/physx_backend.rs
@@ -1,9 +1,7 @@
 #![allow(dead_code)]
 
-use na::{
-    Isometry3, Matrix3, Matrix4, Point3, Quaternion, Rotation3, Translation3, Unit, UnitQuaternion,
-    Vector3,
-};
+use na::{Isometry3, Matrix4, Point3, Quaternion, Translation3, Unit, UnitQuaternion, Vector3};
+use physx::articulation_joint_base::JointMap;
 use physx::cooking::{
     ConvexMeshCookingResult, PxConvexMeshDesc, PxCooking, PxCookingParams, PxHeightFieldDesc,
     PxTriangleMeshDesc, TriangleMeshCookingResult,
@@ -13,15 +11,16 @@ use physx::prelude::*;
 use physx::scene::FrictionType;
 use physx::traits::Class;
 use physx_sys::{
-    FilterShaderCallbackInfo, PxBitAndByte, PxConvexFlags, PxConvexMeshGeometryFlags,
-    PxHeightFieldSample, PxMeshGeometryFlags, PxMeshScale_new, PxRigidActor,
+    FilterShaderCallbackInfo, PxArticulationLink_getInboundJoint, PxBitAndByte, PxConvexFlags,
+    PxConvexMeshGeometryFlags, PxHeightFieldSample, PxMeshGeometryFlags, PxMeshScale_new,
+    PxRigidActor,
 };
 use rapier::counters::Counters;
 use rapier::dynamics::{
-    IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
+    ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyHandle, RigidBodySet,
 };
 use rapier::geometry::{Collider, ColliderSet};
-use rapier::utils::WBasis;
+use rapier::prelude::JointAxesMask;
 use std::collections::HashMap;
 
 trait IntoNa {
@@ -145,7 +144,8 @@ impl PhysxWorld {
         integration_parameters: &IntegrationParameters,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
-        joints: &JointSet,
+        impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
         use_two_friction_directions: bool,
         num_threads: usize,
     ) -> Self {
@@ -181,6 +181,7 @@ impl PhysxWorld {
             let mut scene: Owner<PxScene> = physics.create(scene_desc).unwrap();
             let mut rapier2dynamic = HashMap::new();
             let mut rapier2static = HashMap::new();
+            let mut rapier2link = HashMap::new();
             let cooking_params =
                 PxCookingParams::new(&*physics).expect("Failed to init PhysX cooking.");
             let mut cooking = PxCooking::new(physics.foundation_mut(), &cooking_params)
@@ -192,6 +193,10 @@ impl PhysxWorld {
              *
              */
             for (rapier_handle, rb) in bodies.iter() {
+                if multibody_joints.rigid_body_link(rapier_handle).is_some() {
+                    continue;
+                };
+
                 let pos = rb.position().into_physx();
                 if rb.is_dynamic() {
                     let mut actor = physics.create_dynamic(&pos, rapier_handle).unwrap();
@@ -200,8 +205,10 @@ impl PhysxWorld {
                     actor.set_linear_velocity(&linvel, true);
                     actor.set_angular_velocity(&angvel, true);
                     actor.set_solver_iteration_counts(
-                        integration_parameters.max_position_iterations as u32,
-                        integration_parameters.max_velocity_iterations as u32,
+                        // Use our number of velocity iterations as their number of position iterations.
+                        integration_parameters.max_velocity_iterations.max(1) as u32,
+                        // Use our number of velocity stabilization iterations as their number of velocity iterations.
+                        integration_parameters.max_stabilization_iterations.max(1) as u32,
                     );
 
                     rapier2dynamic.insert(rapier_handle, actor);
@@ -211,6 +218,79 @@ impl PhysxWorld {
                 }
             }
 
+            /*
+             * Articulations.
+             */
+            for multibody in multibody_joints.multibodies() {
+                let mut articulation: Owner<PxArticulationReducedCoordinate> =
+                    physics.create_articulation_reduced_coordinate(()).unwrap();
+                let mut parent = None;
+
+                for link in multibody.links() {
+                    let is_root = parent.is_none();
+                    let rb_handle = link.rigid_body_handle();
+                    let rb = bodies.get(rb_handle).unwrap();
+
+                    if is_root && rb.is_static() {
+                        articulation.set_articulation_flag(ArticulationFlag::FixBase, true);
+                    }
+
+                    let link_pose = rb.position().into_physx();
+                    let px_link = articulation
+                        .create_link(parent.take(), &link_pose, rb_handle)
+                        .unwrap();
+
+                    // TODO: there is no get_inbound_joint_mut?
+                    if let Some(px_inbound_joint) = unsafe {
+                        (PxArticulationLink_getInboundJoint(px_link.as_ptr())
+                            as *mut physx_sys::PxArticulationJointBase
+                            as *mut JointMap)
+                            .as_mut()
+                    } {
+                        let frame1 = link.joint().data.local_frame1.into_physx();
+                        let frame2 = link.joint().data.local_frame2.into_physx();
+
+                        px_inbound_joint.set_parent_pose(&frame1);
+                        px_inbound_joint.set_child_pose(&frame2);
+
+                        /*
+
+                        let px_joint = px_inbound_joint
+                            .as_articulation_joint_reduced_coordinate()
+                            .unwrap();
+
+                        if let Some(_) = link
+                            .articulation()
+                            .downcast_ref::<SphericalMultibodyJoint>()
+                        {
+                            px_joint.set_joint_type(ArticulationJointType::Spherical);
+                            px_joint.set_motion(ArticulationAxis::Swing1, ArticulationMotion::Free);
+                            px_joint.set_motion(ArticulationAxis::Swing2, ArticulationMotion::Free);
+                            px_joint.set_motion(ArticulationAxis::Twist, ArticulationMotion::Free);
+                        } else if let Some(_) =
+                            link.articulation().downcast_ref::<RevoluteMultibodyJoint>()
+                        {
+                            px_joint.set_joint_type(ArticulationJointType::Revolute);
+                            px_joint.set_motion(ArticulationAxis::Swing1, ArticulationMotion::Free);
+                            px_joint.set_motion(ArticulationAxis::Swing2, ArticulationMotion::Free);
+                            px_joint.set_motion(ArticulationAxis::Twist, ArticulationMotion::Free);
+                        }
+
+                         */
+                    }
+
+                    // FIXME: we are using transmute here in order to erase the lifetime of
+                    //        the &mut ref behind px_link (which is tied to the lifetime of the
+                    //        multibody_joint). This looks necessary because we need
+                    //        that mutable ref to create the next link. Yet, the link creation
+                    //        methods also requires a mutable ref to the multibody_joint.
+                    rapier2link.insert(rb_handle, px_link as *mut PxArticulationLink);
+                    parent = Some(unsafe { std::mem::transmute(px_link as *mut _) });
+                }
+
+                scene.add_articulation(articulation);
+            }
+
             /*
              *
              * Colliders
@@ -223,7 +303,15 @@ impl PhysxWorld {
                     if let Some(parent_handle) = collider.parent() {
                         let parent_body = &bodies[parent_handle];
 
-                        if !parent_body.is_dynamic() {
+                        if let Some(link) = rapier2link.get_mut(&parent_handle) {
+                            unsafe {
+                                physx_sys::PxRigidActor_attachShape_mut(
+                                    *link as *mut PxRigidActor,
+                                    px_shape.as_mut_ptr(),
+                                );
+                            }
+                        } else if !parent_body.is_dynamic() {
+                            println!("Ground collider");
                             let actor = rapier2static.get_mut(&parent_handle).unwrap();
                             actor.attach_shape(&mut px_shape);
                         } else {
@@ -246,8 +334,8 @@ impl PhysxWorld {
             }
 
             // Update mass properties and CCD flags.
-            for (rapier_handle, actor) in rapier2dynamic.iter_mut() {
-                let rb = &bodies[*rapier_handle];
+            for (rapier_handle, _rb) in bodies.iter() {
+                let rb = &bodies[rapier_handle];
                 let densities: Vec<_> = rb
                     .colliders()
                     .iter()
@@ -255,8 +343,16 @@ impl PhysxWorld {
                     .collect();
 
                 unsafe {
+                    let actor = if let Some(actor) = rapier2dynamic.get_mut(&rapier_handle) {
+                        std::mem::transmute(actor.as_mut())
+                    } else if let Some(actor) = rapier2link.get_mut(&rapier_handle) {
+                        *actor as *mut _
+                    } else {
+                        continue;
+                    };
+
                     physx_sys::PxRigidBodyExt_updateMassAndInertia_mut(
-                        std::mem::transmute(actor.as_mut()),
+                        actor,
                         densities.as_ptr(),
                         densities.len() as u32,
                         std::ptr::null(),
@@ -265,19 +361,10 @@ impl PhysxWorld {
 
                     if rb.is_ccd_enabled() {
                         physx_sys::PxRigidBody_setRigidBodyFlag_mut(
-                            std::mem::transmute(actor.as_mut()),
+                            actor,
                             RigidBodyFlag::EnableCCD as u32,
                             true,
                         );
-                        // physx_sys::PxRigidBody_setMinCCDAdvanceCoefficient_mut(
-                        //     std::mem::transmute(actor.as_mut()),
-                        //     0.0,
-                        // );
-                        // physx_sys::PxRigidBody_setRigidBodyFlag_mut(
-                        //     std::mem::transmute(actor.as_mut()),
-                        //     RigidBodyFlag::EnableCCDFriction as u32,
-                        //     true,
-                        // );
                     }
                 }
             }
@@ -289,9 +376,10 @@ impl PhysxWorld {
              */
             Self::setup_joints(
                 &mut physics,
-                joints,
+                impulse_joints,
                 &mut rapier2static,
                 &mut rapier2dynamic,
+                &mut rapier2link,
             );
 
             for (_, actor) in rapier2static {
@@ -312,18 +400,22 @@ impl PhysxWorld {
 
     fn setup_joints(
         physics: &mut PxPhysicsFoundation,
-        joints: &JointSet,
+        impulse_joints: &ImpulseJointSet,
         rapier2static: &mut HashMap<RigidBodyHandle, Owner<PxRigidStatic>>,
         rapier2dynamic: &mut HashMap<RigidBodyHandle, Owner<PxRigidDynamic>>,
+        rapier2link: &mut HashMap<RigidBodyHandle, *mut PxArticulationLink>,
     ) {
         unsafe {
-            for joint in joints.iter() {
+            for joint in impulse_joints.iter() {
                 let actor1 = rapier2static
                     .get_mut(&joint.1.body1)
                     .map(|act| &mut **act as *mut PxRigidStatic as *mut PxRigidActor)
                     .or(rapier2dynamic
                         .get_mut(&joint.1.body1)
                         .map(|act| &mut **act as *mut PxRigidDynamic as *mut PxRigidActor))
+                    .or(rapier2link
+                        .get_mut(&joint.1.body1)
+                        .map(|lnk| *lnk as *mut PxRigidActor))
                     .unwrap();
                 let actor2 = rapier2static
                     .get_mut(&joint.1.body2)
@@ -331,150 +423,83 @@ impl PhysxWorld {
                     .or(rapier2dynamic
                         .get_mut(&joint.1.body2)
                         .map(|act| &mut **act as *mut PxRigidDynamic as *mut PxRigidActor))
+                    .or(rapier2link
+                        .get_mut(&joint.1.body2)
+                        .map(|lnk| *lnk as *mut PxRigidActor))
                     .unwrap();
 
-                match &joint.1.params {
-                    JointParams::BallJoint(params) => {
-                        let frame1 = Isometry3::new(params.local_anchor1.coords, na::zero())
-                            .into_physx()
-                            .into();
-                        let frame2 = Isometry3::new(params.local_anchor2.coords, na::zero())
-                            .into_physx()
-                            .into();
+                let px_frame1 = joint.1.data.local_frame1.into_physx();
+                let px_frame2 = joint.1.data.local_frame2.into_physx();
 
-                        physx_sys::phys_PxSphericalJointCreate(
-                            physics.as_mut_ptr(),
-                            actor1,
-                            &frame1 as *const _,
-                            actor2,
-                            &frame2 as *const _,
-                        );
-                    }
-                    JointParams::RevoluteJoint(params) => {
-                        // NOTE: orthonormal_basis() returns the two basis vectors.
-                        // However we only use one and recompute the other just to
-                        // make sure our basis is right-handed.
-                        let basis1a = params.local_axis1.orthonormal_basis()[0];
-                        let basis2a = params.local_axis2.orthonormal_basis()[0];
-                        let basis1b = params.local_axis1.cross(&basis1a);
-                        let basis2b = params.local_axis2.cross(&basis2a);
+                let px_joint = physx_sys::phys_PxD6JointCreate(
+                    physics.as_mut_ptr(),
+                    actor1,
+                    px_frame1.as_ptr(),
+                    actor2,
+                    px_frame2.as_ptr(),
+                );
 
-                        let rotmat1 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[
-                            params.local_axis1.into_inner(),
-                            basis1a,
-                            basis1b,
-                        ]));
-                        let rotmat2 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[
-                            params.local_axis2.into_inner(),
-                            basis2a,
-                            basis2b,
-                        ]));
-                        let axisangle1 = rotmat1.scaled_axis();
-                        let axisangle2 = rotmat2.scaled_axis();
+                let motion_x = if joint.1.data.limit_axes.contains(JointAxesMask::X) {
+                    physx_sys::PxD6Motion::eLIMITED
+                } else if !joint.1.data.locked_axes.contains(JointAxesMask::X) {
+                    physx_sys::PxD6Motion::eFREE
+                } else {
+                    physx_sys::PxD6Motion::eLOCKED
+                };
+                let motion_y = if joint.1.data.limit_axes.contains(JointAxesMask::Y) {
+                    physx_sys::PxD6Motion::eLIMITED
+                } else if !joint.1.data.locked_axes.contains(JointAxesMask::Y) {
+                    physx_sys::PxD6Motion::eFREE
+                } else {
+                    physx_sys::PxD6Motion::eLOCKED
+                };
+                let motion_z = if joint.1.data.limit_axes.contains(JointAxesMask::Z) {
+                    physx_sys::PxD6Motion::eLIMITED
+                } else if !joint.1.data.locked_axes.contains(JointAxesMask::Z) {
+                    physx_sys::PxD6Motion::eFREE
+                } else {
+                    physx_sys::PxD6Motion::eLOCKED
+                };
+                let motion_ax = if joint.1.data.limit_axes.contains(JointAxesMask::ANG_X) {
+                    physx_sys::PxD6Motion::eLIMITED
+                } else if !joint.1.data.locked_axes.contains(JointAxesMask::ANG_X) {
+                    physx_sys::PxD6Motion::eFREE
+                } else {
+                    physx_sys::PxD6Motion::eLOCKED
+                };
+                let motion_ay = if joint.1.data.limit_axes.contains(JointAxesMask::ANG_Y) {
+                    physx_sys::PxD6Motion::eLIMITED
+                } else if !joint.1.data.locked_axes.contains(JointAxesMask::ANG_Y) {
+                    physx_sys::PxD6Motion::eFREE
+                } else {
+                    physx_sys::PxD6Motion::eLOCKED
+                };
+                let motion_az = if joint.1.data.limit_axes.contains(JointAxesMask::ANG_Z) {
+                    physx_sys::PxD6Motion::eLIMITED
+                } else if !joint.1.data.locked_axes.contains(JointAxesMask::ANG_Z) {
+                    physx_sys::PxD6Motion::eFREE
+                } else {
+                    physx_sys::PxD6Motion::eLOCKED
+                };
 
-                        let frame1 = Isometry3::new(params.local_anchor1.coords, axisangle1)
-                            .into_physx()
-                            .into();
-                        let frame2 = Isometry3::new(params.local_anchor2.coords, axisangle2)
-                            .into_physx()
-                            .into();
-
-                        let revolute_joint = physx_sys::phys_PxRevoluteJointCreate(
-                            physics.as_mut_ptr(),
-                            actor1,
-                            &frame1 as *const _,
-                            actor2,
-                            &frame2 as *const _,
-                        );
-
-                        physx_sys::PxRevoluteJoint_setDriveVelocity_mut(
-                            revolute_joint,
-                            params.motor_target_vel,
-                            true,
-                        );
-
-                        if params.motor_damping != 0.0 {
-                            physx_sys::PxRevoluteJoint_setRevoluteJointFlag_mut(
-                                revolute_joint,
-                                physx_sys::PxRevoluteJointFlag::eDRIVE_ENABLED,
-                                true,
-                            );
-                        }
-                    }
-
-                    JointParams::PrismaticJoint(params) => {
-                        // NOTE: orthonormal_basis() returns the two basis vectors.
-                        // However we only use one and recompute the other just to
-                        // make sure our basis is right-handed.
-                        let basis1a = params.local_axis1().orthonormal_basis()[0];
-                        let basis2a = params.local_axis2().orthonormal_basis()[0];
-                        let basis1b = params.local_axis1().cross(&basis1a);
-                        let basis2b = params.local_axis2().cross(&basis2a);
-
-                        let rotmat1 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[
-                            params.local_axis1().into_inner(),
-                            basis1a,
-                            basis1b,
-                        ]));
-                        let rotmat2 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[
-                            params.local_axis2().into_inner(),
-                            basis2a,
-                            basis2b,
-                        ]));
-
-                        let axisangle1 = rotmat1.scaled_axis();
-                        let axisangle2 = rotmat2.scaled_axis();
-
-                        let frame1 = Isometry3::new(params.local_anchor1.coords, axisangle1)
-                            .into_physx()
-                            .into();
-                        let frame2 = Isometry3::new(params.local_anchor2.coords, axisangle2)
-                            .into_physx()
-                            .into();
-
-                        let joint = physx_sys::phys_PxPrismaticJointCreate(
-                            physics.as_mut_ptr(),
-                            actor1,
-                            &frame1 as *const _,
-                            actor2,
-                            &frame2 as *const _,
-                        );
-
-                        if params.limits_enabled {
-                            let limits = physx_sys::PxJointLinearLimitPair {
-                                restitution: 0.0,
-                                bounceThreshold: 0.0,
-                                stiffness: 0.0,
-                                damping: 0.0,
-                                contactDistance: 0.01,
-                                lower: params.limits[0],
-                                upper: params.limits[1],
-                            };
-                            physx_sys::PxPrismaticJoint_setLimit_mut(joint, &limits);
-                            physx_sys::PxPrismaticJoint_setPrismaticJointFlag_mut(
-                                joint,
-                                physx_sys::PxPrismaticJointFlag::eLIMIT_ENABLED,
-                                true,
-                            );
-                        }
-                    }
-                    JointParams::FixedJoint(params) => {
-                        let frame1 = params.local_frame1.into_physx().into();
-                        let frame2 = params.local_frame2.into_physx().into();
-
-                        physx_sys::phys_PxFixedJointCreate(
-                            physics.as_mut_ptr(),
-                            actor1,
-                            &frame1 as *const _,
-                            actor2,
-                            &frame2 as *const _,
-                        );
-                    } // JointParams::GenericJoint(_) => {
-                      //     eprintln!(
-                      //         "Joint type currently unsupported by the PhysX backend: GenericJoint."
-                      //     )
-                      // }
-                }
+                physx_sys::PxD6Joint_setMotion_mut(px_joint, physx_sys::PxD6Axis::eX, motion_x);
+                physx_sys::PxD6Joint_setMotion_mut(px_joint, physx_sys::PxD6Axis::eY, motion_y);
+                physx_sys::PxD6Joint_setMotion_mut(px_joint, physx_sys::PxD6Axis::eZ, motion_z);
+                physx_sys::PxD6Joint_setMotion_mut(
+                    px_joint,
+                    physx_sys::PxD6Axis::eTWIST,
+                    motion_ax,
+                );
+                physx_sys::PxD6Joint_setMotion_mut(
+                    px_joint,
+                    physx_sys::PxD6Axis::eSWING1,
+                    motion_ay,
+                );
+                physx_sys::PxD6Joint_setMotion_mut(
+                    px_joint,
+                    physx_sys::PxD6Axis::eSWING2,
+                    motion_az,
+                );
             }
         }
     }
@@ -497,9 +522,7 @@ impl PhysxWorld {
     }
 
     pub fn sync(&mut self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) {
-        for actor in self.scene.as_mut().unwrap().get_dynamic_actors() {
-            let handle = actor.get_user_data();
-            let pos = actor.get_global_pose().into_na();
+        let mut sync_pos = |handle: &RigidBodyHandle, pos: Isometry3<f32>| {
             let rb = &mut bodies[*handle];
             rb.set_position(pos, false);
 
@@ -509,6 +532,22 @@ impl PhysxWorld {
                     pos * collider.position_wrt_parent().copied().unwrap_or(na::one()),
                 );
             }
+        };
+
+        for actor in self.scene.as_mut().unwrap().get_dynamic_actors() {
+            let handle = actor.get_user_data();
+            let pos = actor.get_global_pose().into_na();
+            sync_pos(handle, pos);
+        }
+
+        for articulation in self.scene.as_mut().unwrap().get_articulations() {
+            if let Some(articulation) = articulation.as_articulation_reduced_coordinate() {
+                for link in articulation.get_links() {
+                    let handle = link.get_user_data();
+                    let pos = link.get_global_pose().into_na();
+                    sync_pos(handle, pos);
+                }
+            }
         }
     }
 }
@@ -673,7 +712,7 @@ fn physx_collider_from_rapier_collider(
 type PxPhysicsFoundation = PhysicsFoundation<DefaultAllocator, PxShape>;
 type PxMaterial = physx::material::PxMaterial<()>;
 type PxShape = physx::shape::PxShape<(), PxMaterial>;
-type PxArticulationLink = physx::articulation_link::PxArticulationLink<(), PxShape>;
+type PxArticulationLink = physx::articulation_link::PxArticulationLink<RigidBodyHandle, PxShape>;
 type PxRigidStatic = physx::rigid_static::PxRigidStatic<(), PxShape>;
 type PxRigidDynamic = physx::rigid_dynamic::PxRigidDynamic<RigidBodyHandle, PxShape>;
 type PxArticulation = physx::articulation::PxArticulation<(), PxArticulationLink>;
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 706a40a..ac53733 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -11,7 +11,8 @@ use crate::{graphics::GraphicsManager, harness::RunState};
 
 use na::{self, Point2, Point3, Vector3};
 use rapier::dynamics::{
-    IntegrationParameters, JointSet, RigidBodyActivation, RigidBodyHandle, RigidBodySet,
+    ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyActivation,
+    RigidBodyHandle, RigidBodySet,
 };
 use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
 #[cfg(feature = "dim3")]
@@ -22,8 +23,6 @@ use rapier::pipeline::PhysicsHooks;
 #[cfg(all(feature = "dim2", feature = "other-backends"))]
 use crate::box2d_backend::Box2dWorld;
 use crate::harness::Harness;
-#[cfg(feature = "other-backends")]
-use crate::nphysics_backend::NPhysicsWorld;
 #[cfg(all(feature = "dim3", feature = "other-backends"))]
 use crate::physx_backend::PhysxWorld;
 use bevy::render::camera::Camera;
@@ -38,12 +37,10 @@ use crate::camera3d::{OrbitCamera, OrbitCameraPlugin};
 use bevy::render::pipeline::PipelineDescriptor;
 
 const RAPIER_BACKEND: usize = 0;
-#[cfg(feature = "other-backends")]
-const NPHYSICS_BACKEND: usize = 1;
 #[cfg(all(feature = "dim2", feature = "other-backends"))]
-const BOX2D_BACKEND: usize = 2;
-pub(crate) const PHYSX_BACKEND_PATCH_FRICTION: usize = 2;
-pub(crate) const PHYSX_BACKEND_TWO_FRICTION_DIR: usize = 3;
+const BOX2D_BACKEND: usize = 1;
+pub(crate) const PHYSX_BACKEND_PATCH_FRICTION: usize = 1;
+pub(crate) const PHYSX_BACKEND_TWO_FRICTION_DIR: usize = 2;
 
 #[derive(PartialEq)]
 pub enum RunMode {
@@ -128,7 +125,6 @@ struct OtherBackends {
     box2d: Option<Box2dWorld>,
     #[cfg(feature = "dim3")]
     physx: Option<PhysxWorld>,
-    nphysics: Option<NPhysicsWorld>,
 }
 struct Plugins(Vec<Box<dyn TestbedPlugin>>);
 
@@ -169,8 +165,6 @@ impl TestbedApp {
 
         #[allow(unused_mut)]
         let mut backend_names = vec!["rapier"];
-        #[cfg(feature = "other-backends")]
-        backend_names.push("nphysics");
         #[cfg(all(feature = "dim2", feature = "other-backends"))]
         backend_names.push("box2d");
         #[cfg(all(feature = "dim3", feature = "other-backends"))]
@@ -207,7 +201,6 @@ impl TestbedApp {
             box2d: None,
             #[cfg(feature = "dim3")]
             physx: None,
-            nphysics: None,
         };
 
         TestbedApp {
@@ -271,28 +264,15 @@ impl TestbedApp {
                 for (backend_id, backend) in backend_names.iter().enumerate() {
                     println!("|_ using backend {}", backend);
                     self.state.selected_backend = backend_id;
-                    if cfg!(feature = "dim3")
-                        && (backend_id == PHYSX_BACKEND_PATCH_FRICTION
-                            || backend_id == PHYSX_BACKEND_TWO_FRICTION_DIR)
-                    {
-                        self.harness
-                            .physics
-                            .integration_parameters
-                            .max_velocity_iterations = 1;
-                        self.harness
-                            .physics
-                            .integration_parameters
-                            .max_position_iterations = 4;
-                    } else {
-                        self.harness
-                            .physics
-                            .integration_parameters
-                            .max_velocity_iterations = 4;
-                        self.harness
-                            .physics
-                            .integration_parameters
-                            .max_position_iterations = 1;
-                    }
+                    self.harness
+                        .physics
+                        .integration_parameters
+                        .max_velocity_iterations = 4;
+                    self.harness
+                        .physics
+                        .integration_parameters
+                        .max_stabilization_iterations = 1;
+
                     // Init world.
                     let mut testbed = Testbed {
                         graphics: None,
@@ -341,20 +321,6 @@ impl TestbedApp {
                                     );
                                 }
                             }
-
-                            #[cfg(feature = "other-backends")]
-                            {
-                                if self.state.selected_backend == NPHYSICS_BACKEND {
-                                    self.other_backends.nphysics.as_mut().unwrap().step(
-                                        &mut self.harness.physics.pipeline.counters,
-                                        &self.harness.physics.integration_parameters,
-                                    );
-                                    self.other_backends.nphysics.as_mut().unwrap().sync(
-                                        &mut self.harness.physics.bodies,
-                                        &mut self.harness.physics.colliders,
-                                    );
-                                }
-                            }
                         }
 
                         // Skip the first update.
@@ -498,20 +464,40 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> {
         &mut self.harness
     }
 
-    pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) {
-        self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ())
+    pub fn set_world(
+        &mut self,
+        bodies: RigidBodySet,
+        colliders: ColliderSet,
+        impulse_joints: ImpulseJointSet,
+        multibody_joints: MultibodyJointSet,
+    ) {
+        self.set_world_with_params(
+            bodies,
+            colliders,
+            impulse_joints,
+            multibody_joints,
+            Vector::y() * -9.81,
+            (),
+        )
     }
 
     pub fn set_world_with_params(
         &mut self,
         bodies: RigidBodySet,
         colliders: ColliderSet,
-        joints: JointSet,
+        impulse_joints: ImpulseJointSet,
+        multibody_joints: MultibodyJointSet,
         gravity: Vector<f32>,
         hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
     ) {
-        self.harness
-            .set_world_with_params(bodies, colliders, joints, gravity, hooks);
+        self.harness.set_world_with_params(
+            bodies,
+            colliders,
+            impulse_joints,
+            multibody_joints,
+            gravity,
+            hooks,
+        );
 
         self.state
             .action_flags
@@ -526,7 +512,7 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> {
                     self.harness.physics.gravity,
                     &self.harness.physics.bodies,
                     &self.harness.physics.colliders,
-                    &self.harness.physics.joints,
+                    &self.harness.physics.impulse_joints,
                 ));
             }
         }
@@ -541,24 +527,13 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> {
                     &self.harness.physics.integration_parameters,
                     &self.harness.physics.bodies,
                     &self.harness.physics.colliders,
-                    &self.harness.physics.joints,
+                    &self.harness.physics.impulse_joints,
+                    &self.harness.physics.multibody_joints,
                     self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR,
                     self.harness.state.num_threads,
                 ));
             }
         }
-
-        #[cfg(feature = "other-backends")]
-        {
-            if self.state.selected_backend == NPHYSICS_BACKEND {
-                self.other_backends.nphysics = Some(NPhysicsWorld::from_rapier(
-                    self.harness.physics.gravity,
-                    &self.harness.physics.bodies,
-                    &self.harness.physics.colliders,
-                    &self.harness.physics.joints,
-                ));
-            }
-        }
     }
 
     #[cfg(feature = "dim2")]
@@ -634,85 +609,134 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> {
         self.plugins.0.push(Box::new(plugin));
     }
 
-    // fn handle_common_event<'b>(&mut self, event: Event<'b>) -> Event<'b> {
-    //     match event.value {
-    //         WindowEvent::Key(Key::T, Action::Release, _) => {
-    //             if self.state.running == RunMode::Stop {
-    //                 self.state.running = RunMode::Running;
-    //             } else {
-    //                 self.state.running = RunMode::Stop;
-    //             }
-    //         }
-    //         WindowEvent::Key(Key::S, Action::Release, _) => self.state.running = RunMode::Step,
-    //         WindowEvent::Key(Key::R, Action::Release, _) => self
-    //             .state
-    //             .action_flags
-    //             .set(TestbedActionFlags::EXAMPLE_CHANGED, true),
-    //         WindowEvent::Key(Key::C, Action::Release, _) => {
-    //             // Delete 1 collider of 10% of the remaining dynamic bodies.
-    //             let mut colliders: Vec<_> = self
-    //                 .harness
-    //                 .physics
-    //                 .bodies
-    //                 .iter()
-    //                 .filter(|e| e.1.is_dynamic())
-    //                 .filter(|e| !e.1.colliders().is_empty())
-    //                 .map(|e| e.1.colliders().to_vec())
-    //                 .collect();
-    //             colliders.sort_by_key(|co| -(co.len() as isize));
-    //
-    //             let num_to_delete = (colliders.len() / 10).max(1);
-    //             for to_delete in &colliders[..num_to_delete] {
-    //                 self.harness.physics.colliders.remove(
-    //                     to_delete[0],
-    //                     &mut self.harness.physics.islands,
-    //                     &mut self.harness.physics.bodies,
-    //                     true,
-    //                 );
-    //             }
-    //         }
-    //         WindowEvent::Key(Key::D, Action::Release, _) => {
-    //             // Delete 10% of the remaining dynamic bodies.
-    //             let dynamic_bodies: Vec<_> = self
-    //                 .harness
-    //                 .physics
-    //                 .bodies
-    //                 .iter()
-    //                 .filter(|e| !e.1.is_static())
-    //                 .map(|e| e.0)
-    //                 .collect();
-    //             let num_to_delete = (dynamic_bodies.len() / 10).max(1);
-    //             for to_delete in &dynamic_bodies[..num_to_delete] {
-    //                 self.harness.physics.bodies.remove(
-    //                     *to_delete,
-    //                     &mut self.harness.physics.islands,
-    //                     &mut self.harness.physics.colliders,
-    //                     &mut self.harness.physics.joints,
-    //                 );
-    //             }
-    //         }
-    //         WindowEvent::Key(Key::J, Action::Release, _) => {
-    //             // Delete 10% of the remaining joints.
-    //             let joints: Vec<_> = self.harness.physics.joints.iter().map(|e| e.0).collect();
-    //             let num_to_delete = (joints.len() / 10).max(1);
-    //             for to_delete in &joints[..num_to_delete] {
-    //                 self.harness.physics.joints.remove(
-    //                     *to_delete,
-    //                     &mut self.harness.physics.islands,
-    //                     &mut self.harness.physics.bodies,
-    //                     true,
-    //                 );
-    //             }
-    //         }
-    //         WindowEvent::CursorPos(x, y, _) => {
-    //             self.state.cursor_pos.x = x as f32;
-    //             self.state.cursor_pos.y = y as f32;
-    //         }
-    //         _ => {}
-    //     }
-    //
-    //     event
-    // }
+    fn handle_common_events(&mut self, events: &Input<KeyCode>) {
+        for key in events.get_just_released() {
+            match *key {
+                KeyCode::T => {
+                    if self.state.running == RunMode::Stop {
+                        self.state.running = RunMode::Running;
+                    } else {
+                        self.state.running = RunMode::Stop;
+                    }
+                }
+                KeyCode::S => self.state.running = RunMode::Step,
+                KeyCode::R => self
+                    .state
+                    .action_flags
+                    .set(TestbedActionFlags::EXAMPLE_CHANGED, true),
+                KeyCode::C => {
+                    // Delete 1 collider of 10% of the remaining dynamic bodies.
+                    let mut colliders: Vec<_> = self
+                        .harness
+                        .physics
+                        .bodies
+                        .iter()
+                        .filter(|e| e.1.is_dynamic())
+                        .filter(|e| !e.1.colliders().is_empty())
+                        .map(|e| e.1.colliders().to_vec())
+                        .collect();
+                    colliders.sort_by_key(|co| -(co.len() as isize));
+
+                    let num_to_delete = (colliders.len() / 10).max(1);
+                    for to_delete in &colliders[..num_to_delete] {
+                        if let Some(graphics) = self.graphics.as_mut() {
+                            graphics.remove_collider(to_delete[0], &self.harness.physics.colliders);
+                        }
+                        self.harness.physics.colliders.remove(
+                            to_delete[0],
+                            &mut self.harness.physics.islands,
+                            &mut self.harness.physics.bodies,
+                            true,
+                        );
+                    }
+                }
+                KeyCode::D => {
+                    // Delete 10% of the remaining dynamic bodies.
+                    let dynamic_bodies: Vec<_> = self
+                        .harness
+                        .physics
+                        .bodies
+                        .iter()
+                        .filter(|e| !e.1.is_static())
+                        .map(|e| e.0)
+                        .collect();
+                    let num_to_delete = (dynamic_bodies.len() / 10).max(1);
+                    for to_delete in &dynamic_bodies[..num_to_delete] {
+                        if let Some(graphics) = self.graphics.as_mut() {
+                            graphics.remove_body(*to_delete);
+                        }
+                        self.harness.physics.bodies.remove(
+                            *to_delete,
+                            &mut self.harness.physics.islands,
+                            &mut self.harness.physics.colliders,
+                            &mut self.harness.physics.impulse_joints,
+                            &mut self.harness.physics.multibody_joints,
+                        );
+                    }
+                }
+                KeyCode::J => {
+                    // Delete 10% of the remaining impulse_joints.
+                    let impulse_joints: Vec<_> = self
+                        .harness
+                        .physics
+                        .impulse_joints
+                        .iter()
+                        .map(|e| e.0)
+                        .collect();
+                    let num_to_delete = (impulse_joints.len() / 10).max(1);
+                    for to_delete in &impulse_joints[..num_to_delete] {
+                        self.harness.physics.impulse_joints.remove(
+                            *to_delete,
+                            &mut self.harness.physics.islands,
+                            &mut self.harness.physics.bodies,
+                            true,
+                        );
+                    }
+                }
+                KeyCode::A => {
+                    // Delete 10% of the remaining multibody_joints.
+                    let multibody_joints: Vec<_> = self
+                        .harness
+                        .physics
+                        .multibody_joints
+                        .iter()
+                        .map(|e| e.0)
+                        .collect();
+                    let num_to_delete = (multibody_joints.len() / 10).max(1);
+                    for to_delete in &multibody_joints[..num_to_delete] {
+                        self.harness.physics.multibody_joints.remove(
+                            *to_delete,
+                            &mut self.harness.physics.islands,
+                            &mut self.harness.physics.bodies,
+                            true,
+                        );
+                    }
+                }
+                KeyCode::M => {
+                    // Delete one remaining multibody.
+                    let to_delete = self
+                        .harness
+                        .physics
+                        .multibody_joints
+                        .iter()
+                        .next()
+                        .map(|a| a.2.rigid_body_handle());
+                    if let Some(to_delete) = to_delete {
+                        self.harness
+                            .physics
+                            .multibody_joints
+                            .remove_multibody_articulations(
+                                to_delete,
+                                &mut self.harness.physics.islands,
+                                &mut self.harness.physics.bodies,
+                                true,
+                            );
+                    }
+                }
+                _ => {}
+            }
+        }
+    }
 
     // #[cfg(feature = "dim2")]
     // fn handle_special_event(&mut self) {}
@@ -878,11 +902,37 @@ fn update_testbed(
     ui_context: Res<EguiContext>,
     mut gfx_components: Query<(&mut Transform,)>,
     mut cameras: Query<(&Camera, &GlobalTransform, &mut OrbitCamera)>,
+    keys: Res<Input<KeyCode>>,
 ) {
     let meshes = &mut *meshes;
     let materials = &mut *materials;
     let prev_example = state.selected_example;
 
+    // Handle inputs
+    {
+        let graphics_context = TestbedGraphics {
+            pipelines: &mut *pipelines,
+            shaders: &mut *shaders,
+            graphics: &mut *graphics,
+            commands: &mut commands,
+            meshes: &mut *meshes,
+            materials: &mut *materials,
+            components: &mut gfx_components,
+            camera: &mut cameras.iter_mut().next().unwrap().2,
+        };
+
+        let mut testbed = Testbed {
+            graphics: Some(graphics_context),
+            state: &mut *state,
+            harness: &mut *harness,
+            #[cfg(feature = "other-backends")]
+            other_backends: &mut *other_backends,
+            plugins: &mut *plugins,
+        };
+
+        testbed.handle_common_events(&*keys);
+    }
+
     // Update UI
     {
         let harness = &mut *harness;
@@ -943,28 +993,6 @@ fn update_testbed(
 
             if state.selected_example != prev_example {
                 harness.physics.integration_parameters = IntegrationParameters::default();
-
-                if cfg!(feature = "dim3")
-                    && (state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION
-                        || state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR)
-                {
-                    let max_position_iterations = harness
-                        .physics
-                        .integration_parameters
-                        .max_position_iterations;
-                    let max_velocity_iterations = harness
-                        .physics
-                        .integration_parameters
-                        .max_velocity_iterations;
-                    harness
-                        .physics
-                        .integration_parameters
-                        .max_velocity_iterations = max_position_iterations;
-                    harness
-                        .physics
-                        .integration_parameters
-                        .max_position_iterations = max_velocity_iterations;
-                }
             }
 
             let selected_example = state.selected_example;
@@ -1009,7 +1037,7 @@ fn update_testbed(
                 &harness.physics.narrow_phase,
                 &harness.physics.bodies,
                 &harness.physics.colliders,
-                &harness.physics.joints,
+                &harness.physics.impulse_joints,
             )
             .ok();
 
@@ -1172,22 +1200,6 @@ fn update_testbed(
                 }
             }
 
-            #[cfg(feature = "other-backends")]
-            {
-                if state.selected_backend == NPHYSICS_BACKEND {
-                    let harness = &mut *harness;
-                    other_backends.nphysics.as_mut().unwrap().step(
-                        &mut harness.physics.pipeline.counters,
-                        &harness.physics.integration_parameters,
-                    );
-                    other_backends
-                        .nphysics
-                        .as_mut()
-                        .unwrap()
-                        .sync(&mut harness.physics.bodies, &mut harness.physics.colliders);
-                }
-            }
-
             for plugin in &mut plugins.0 {
                 plugin.run_callbacks(&mut harness);
             }
diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs
index 4a21072..894b6a4 100644
--- a/src_testbed/ui.rs
+++ b/src_testbed/ui.rs
@@ -1,7 +1,10 @@
 use rapier::counters::Counters;
 
 use crate::harness::Harness;
-use crate::testbed::{RunMode, TestbedActionFlags, TestbedState, TestbedStateFlags};
+use crate::testbed::{
+    RunMode, TestbedActionFlags, TestbedState, TestbedStateFlags, PHYSX_BACKEND_PATCH_FRICTION,
+    PHYSX_BACKEND_TWO_FRICTION_DIR,
+};
 
 use crate::PhysicsState;
 use bevy_egui::egui::Slider;
@@ -10,8 +13,6 @@ use bevy_egui::{egui, EguiContext};
 pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &mut Harness) {
     egui::Window::new("Parameters").show(ui_context.ctx(), |ui| {
         if state.backend_names.len() > 1 && !state.example_names.is_empty() {
-            #[cfg(all(feature = "dim3", feature = "other-backends"))]
-            let prev_selected_backend = state.selected_backend;
             let mut changed = false;
             egui::ComboBox::from_label("backend")
                 .width(150.0)
@@ -29,30 +30,6 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
                 state
                     .action_flags
                     .set(TestbedActionFlags::BACKEND_CHANGED, true);
-
-                #[cfg(all(feature = "dim3", feature = "other-backends"))]
-                fn is_physx(id: usize) -> bool {
-                    id == crate::testbed::PHYSX_BACKEND_PATCH_FRICTION
-                        || id == crate::testbed::PHYSX_BACKEND_TWO_FRICTION_DIR
-                }
-
-                #[cfg(all(feature = "dim3", feature = "other-backends"))]
-                if (is_physx(state.selected_backend) && !is_physx(prev_selected_backend))
-                    || (!is_physx(state.selected_backend) && is_physx(prev_selected_backend))
-                {
-                    // PhysX defaults (4 position iterations, 1 velocity) are the
-                    // opposite of rapier's (4 velocity iterations, 1 position).
-                    std::mem::swap(
-                        &mut harness
-                            .physics
-                            .integration_parameters
-                            .max_position_iterations,
-                        &mut harness
-                            .physics
-                            .integration_parameters
-                            .max_velocity_iterations,
-                    );
-                }
             }
 
             ui.separator();
@@ -113,14 +90,47 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
         });
 
         let integration_parameters = &mut harness.physics.integration_parameters;
-        ui.add(
-            Slider::new(&mut integration_parameters.max_velocity_iterations, 0..=200)
-                .text("vels. iters."),
-        );
-        ui.add(
-            Slider::new(&mut integration_parameters.max_position_iterations, 0..=200)
-                .text("pos. iters."),
+
+        ui.checkbox(
+            &mut integration_parameters.interleave_restitution_and_friction_resolution,
+            "interleave friction resolution",
         );
+
+        if state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION
+            || state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR
+        {
+            ui.add(
+                Slider::new(&mut integration_parameters.max_velocity_iterations, 0..=200)
+                    .text("pos. iters."),
+            );
+            ui.add(
+                Slider::new(
+                    &mut integration_parameters.max_stabilization_iterations,
+                    0..=200,
+                )
+                .text("vel. iters."),
+            );
+        } else {
+            ui.add(
+                Slider::new(&mut integration_parameters.max_velocity_iterations, 0..=200)
+                    .text("vel. rest. iters."),
+            );
+            ui.add(
+                Slider::new(
+                    &mut integration_parameters.max_velocity_friction_iterations,
+                    0..=200,
+                )
+                .text("vel. frict. iters."),
+            );
+            ui.add(
+                Slider::new(
+                    &mut integration_parameters.max_stabilization_iterations,
+                    0..=200,
+                )
+                .text("vel. stab. iters."),
+            );
+        }
+
         #[cfg(feature = "parallel")]
         {
             ui.add(
@@ -135,10 +145,6 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
             Slider::new(&mut integration_parameters.min_island_size, 1..=10_000)
                 .text("min island size"),
         );
-        ui.add(
-            Slider::new(&mut integration_parameters.warmstart_coeff, 0.0..=1.0)
-                .text("warmstart coeff"),
-        );
         let mut frequency = integration_parameters.inv_dt().round() as u32;
         ui.add(Slider::new(&mut frequency, 0..=240).text("frequency (Hz)"));
         integration_parameters.set_inv_dt(frequency as f32);
@@ -247,7 +253,7 @@ fn serialization_string(timestep_id: usize, physics: &PhysicsState) -> String {
     let cs = bincode::serialize(&physics.colliders).unwrap();
     // println!("cs: {}", instant::now() - t);
     // let t = instant::now();
-    let js = bincode::serialize(&physics.joints).unwrap();
+    let js = bincode::serialize(&physics.impulse_joints).unwrap();
     // println!("js: {}", instant::now() - t);
     let serialization_time = instant::now() - t;
     let hash_bf = md5::compute(&bf);