First public release of Rapier.

.github/FUNDING.yml

@@ -0,0 +1,12 @@
+# These are supported funding model platforms
+
+github: [ "dimforge" ] # Replace with up to 4 GitHub Sponsors-enabled usernames e.g., [user1, user2]
+patreon: # Replace with a single Patreon username
+open_collective: # Replace with a single Open Collective username
+ko_fi: # Replace with a single Ko-fi username
+tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel
+community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry
+liberapay: # Replace with a single Liberapay username
+issuehunt: # Replace with a single IssueHunt username
+otechie: # Replace with a single Otechie username
+custom: # Replace with up to 4 custom sponsorship URLs e.g., ['link1', 'link2']

.gitignore

@@ -0,0 +1,8 @@
+**/*.rs.bk
+Cargo.lock
+node_modules
+target
+.idea
+.DS_Store
+package-lock.json
+**/*.csv

Cargo.toml

@@ -0,0 +1,12 @@
+[workspace]
+members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "examples2d/wasm",
+            "build/rapier3d", "build/rapier_testbed3d", "examples3d", "examples3d/wasm",]
+
+[patch.crates-io]
+#wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" }
+
+[profile.release]
+debug = false
+codegen-units = 1
+#opt-level = 1
+#lto = true

LICENSE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright 2020 Sébastien Crozet
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

README.md

@@ -0,0 +1,30 @@
+<p align="center">
+  <img src="https://www.rapier.rs/img/rapier_logo_color_textpath.svg" alt="crates.io">
+</p>
+<p align="center">
+    <a href="https://discord.gg/vt9DJSW">
+        <img src="https://img.shields.io/discord/507548572338880513.svg?logo=discord&colorB=7289DA">
+    </a>
+    <a href="https://travis-ci.org/dimforge/rapier">
+        <img src="https://travis-ci.org/dimforge/rapier.svg?branch=master" alt="Build status">
+    </a>
+    <a href="https://crates.io/crates/rapier">
+         <img src="https://meritbadge.herokuapp.com/rapier?style=flat-square" alt="crates.io">
+    </a>
+    <a href="https://opensource.org/licenses/Apache-2.0">
+        <img src="https://img.shields.io/badge/License-Apache%202.0-blue.svg">
+    </a>
+</p>
+<p align = "center">
+    <strong>
+        <a href="https://rapier.rs">Website</a> | <a href="https://rapier.rs/docs/">Documentation</a>
+</p>
+
+-----
+
+<p align = "center">
+<b>2D and 3D physics engines</b>
+<i>for the Rust programming language.</i>
+</p>
+
+-----

build/rapier2d/Cargo.toml

@@ -0,0 +1,53 @@
+# Name idea: bident for 2D and trident for 3D
+[package]
+name    = "rapier2d"
+version = "0.1.0"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+description = "2-dimensional physics engine in Rust."
+documentation = "http://rapier.rs/rustdoc/rapier2d/index.html"
+homepage = "http://rapier.rs"
+repository = "https://github.com/rustsim/rapier"
+readme = "README.md"
+keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+license = "Apache-2.0"
+edition = "2018"
+
+[features]
+default = [ "dim2" ]
+dim2    = [ ]
+parallel = [ "rayon" ]
+simd-stable = [ "simba/wide", "simd-is-enabled" ]
+simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
+# Do not enable this feature directly. It is automatically
+# enabled with the "simd-stable" or "simd-nightly" feature.
+simd-is-enabled = [ ]
+wasm-bindgen = [ "instant/wasm-bindgen" ]
+serde-serialize = [ "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
+enhanced-determinism = [ "simba/libm_force", "indexmap" ]
+
+[lib]
+name = "rapier2d"
+path = "../../src/lib.rs"
+required-features = [ "dim2" ]
+
+
+[dependencies]
+vec_map = "0.8"
+instant = { version = "0.1", features = [ "now" ]}
+num-traits = "0.2"
+nalgebra = "0.22"
+ncollide2d = "0.24"
+simba = "0.2"
+approx = "0.3"
+rayon = { version = "1", optional = true }
+crossbeam = "0.7"
+generational-arena = "0.2"
+arrayvec = "0.5"
+bit-vec = "0.6"
+rustc-hash = "1"
+serde = { version = "1", features = [ "derive" ], optional = true }
+indexmap = { version = "1", features = [ "serde-1" ], optional = true }
+
+[dev-dependencies]
+bincode = "1"
+serde = { version = "1", features = [ "derive" ] }

build/rapier3d/Cargo.toml

@@ -0,0 +1,53 @@
+# Name idea: bident for 2D and trident for 3D
+[package]
+name    = "rapier3d"
+version = "0.1.0"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+description = "3-dimensional physics engine in Rust."
+documentation = "http://rapier.rs/rustdoc/rapier2d/index.html"
+homepage = "http://rapier.rs"
+repository = "https://github.com/rustsim/rapier"
+readme = "README.md"
+keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+license = "Apache-2.0"
+edition = "2018"
+
+[features]
+default = [ "dim3" ]
+dim3    = [ ]
+parallel = [ "rayon" ]
+simd-stable = [ "simba/wide", "simd-is-enabled" ]
+simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
+# Do not enable this feature directly. It is automatically
+# enabled with the "simd-stable" or "simd-nightly" feature.
+simd-is-enabled = [ ]
+wasm-bindgen = [ "instant/wasm-bindgen" ]
+serde-serialize = [ "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
+enhanced-determinism = [ "simba/libm_force", "indexmap" ]
+
+[lib]
+name = "rapier3d"
+path = "../../src/lib.rs"
+required-features = [ "dim3" ]
+
+
+[dependencies]
+vec_map = "0.8"
+instant = { version = "0.1", features = [ "now" ]}
+num-traits = "0.2"
+nalgebra = "0.22"
+ncollide3d = "0.24"
+simba = "0.2"
+approx = "0.3"
+rayon = { version = "1", optional = true }
+crossbeam = "0.7"
+generational-arena = "0.2"
+arrayvec = "0.5"
+bit-vec = "0.6"
+rustc-hash = "1"
+serde = { version = "1", features = [ "derive" ], optional = true }
+indexmap = { version = "1", features = [ "serde-1" ], optional = true }
+
+[dev-dependencies]
+bincode = "1"
+serde = { version = "1", features = [ "derive" ] }

build/rapier_testbed2d/Cargo.toml

@@ -0,0 +1,45 @@
+[package]
+name    = "rapier_testbed2d"
+version = "0.1.0"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+description = "Testbed for the 2-dimensional physics engine in Rust."
+homepage = "http://rapier.org"
+repository = "https://github.com/rustsim/rapier"
+keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+license = "Apache-2.0"
+edition = "2018"
+
+
+[lib]
+name = "rapier_testbed2d"
+path = "../../src_testbed/lib.rs"
+required-features = [ "dim2" ]
+
+[features]
+default = [ "dim2" ]
+dim2 = [ ]
+parallel = [ "rapier2d/parallel", "num_cpus" ]
+other-backends = [ "wrapped2d", "nphysics2d" ]
+
+
+[dependencies]
+nalgebra   = "0.22"
+kiss3d     = { version = "0.25", features = [ "conrod" ] }
+rand       = "0.7"
+rand_pcg   = "0.2"
+instant    = { version = "0.1", features = [ "web-sys", "now" ]}
+bitflags   = "1"
+num_cpus   = { version = "1", optional = true }
+wrapped2d  = { version = "0.4", optional = true }
+ncollide2d = "0.24"
+nphysics2d = { version = "0.17", optional = true }
+crossbeam = "0.7"
+bincode = "1"
+flexbuffers = "0.1"
+md5 = "0.7"
+
+
+[dependencies.rapier2d]
+path = "../rapier2d"
+version = "0.1"
+features = [ "serde-serialize" ]

build/rapier_testbed3d/Cargo.toml

@@ -0,0 +1,47 @@
+[package]
+name    = "rapier_testbed3d"
+version = "0.1.0"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+description = "Testbed for the 3-dimensional physics engine in Rust."
+homepage = "http://rapier.org"
+repository = "https://github.com/rustsim/rapier"
+keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+license = "Apache-2.0"
+edition = "2018"
+
+
+[lib]
+name = "rapier_testbed3d"
+path = "../../src_testbed/lib.rs"
+required-features = [ "dim3" ]
+
+[features]
+default = [ "dim3" ]
+dim3 = [ ]
+parallel = [ "rapier3d/parallel", "num_cpus" ]
+other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ]
+
+[dependencies]
+nalgebra   = "0.22"
+kiss3d     = { version = "0.25", features = [ "conrod" ] }
+rand       = "0.7"
+rand_pcg   = "0.2"
+instant    = { version = "0.1", features = [ "web-sys", "now" ]}
+bitflags   = "1"
+glam       = { version = "0.8", optional = true }
+num_cpus   = { version = "1", optional = true }
+ncollide3d = "0.24"
+nphysics3d = { version = "0.17", optional = true }
+physx      = { version = "0.6", optional = true }
+physx-sys = { version = "0.4", optional = true }
+crossbeam = "0.7"
+bincode = "1"
+flexbuffers = "0.1"
+serde_cbor = "0.11"
+md5 = "0.7"
+serde = { version = "1", features = [ "derive" ] }
+
+[dependencies.rapier3d]
+path = "../rapier3d"
+version = "0.1"
+features = [ "serde-serialize" ]

examples2d/Cargo.toml

@@ -0,0 +1,27 @@
+[package]
+name    = "nphysics-examples-2d"
+version = "0.1.0"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+edition = "2018"
+
+[features]
+parallel = [ "rapier2d/parallel", "rapier_testbed2d/parallel" ]
+simd-stable = [ "rapier2d/simd-stable" ]
+simd-nightly = [ "rapier2d/simd-nightly" ]
+other-backends = [ "rapier_testbed2d/other-backends" ]
+enhanced-determinism = [ "rapier2d/enhanced-determinism" ]
+
+[dependencies]
+rand       = "0.7"
+Inflector  = "0.11"
+nalgebra = "0.22"
+
+[dependencies.rapier_testbed2d]
+path = "../build/rapier_testbed2d"
+
+[dependencies.rapier2d]
+path = "../build/rapier2d"
+
+[[bin]]
+name = "all_examples2"
+path = "./all_examples2.rs"

examples2d/all_examples2.rs

@@ -0,0 +1,90 @@
+#![allow(dead_code)]
+
+extern crate nalgebra as na;
+
+#[cfg(target_arch = "wasm32")]
+use wasm_bindgen::prelude::*;
+
+use inflector::Inflector;
+
+use rapier_testbed2d::Testbed;
+use std::cmp::Ordering;
+
+mod balls2;
+mod boxes2;
+mod capsules2;
+mod debug_box_ball2;
+mod heightfield2;
+mod joints2;
+mod kinematic2;
+mod pyramid2;
+mod sensor2;
+mod stress_joint_ball2;
+mod stress_joint_fixed2;
+mod stress_joint_prismatic2;
+
+fn demo_name_from_command_line() -> Option<String> {
+    let mut args = std::env::args();
+
+    while let Some(arg) = args.next() {
+        if &arg[..] == "--example" {
+            return args.next();
+        }
+    }
+
+    None
+}
+
+#[cfg(any(target_arch = "wasm32", target_arch = "asmjs"))]
+fn demo_name_from_url() -> Option<String> {
+    None
+    //    let window = stdweb::web::window();
+    //    let hash = window.location()?.search().ok()?;
+    //    Some(hash[1..].to_string())
+}
+
+#[cfg(not(any(target_arch = "wasm32", target_arch = "asmjs")))]
+fn demo_name_from_url() -> Option<String> {
+    None
+}
+
+#[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))]
+pub fn main() {
+    let demo = demo_name_from_command_line()
+        .or_else(|| demo_name_from_url())
+        .unwrap_or(String::new())
+        .to_camel_case();
+
+    let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
+        ("Balls", balls2::init_world),
+        ("Boxes", boxes2::init_world),
+        ("Capsules", capsules2::init_world),
+        ("Heightfield", heightfield2::init_world),
+        ("Joints", joints2::init_world),
+        ("Kinematic", kinematic2::init_world),
+        ("Pyramid", pyramid2::init_world),
+        ("Sensor", sensor2::init_world),
+        ("(Debug) box ball", debug_box_ball2::init_world),
+        ("(Stress test) joint ball", stress_joint_ball2::init_world),
+        ("(Stress test) joint fixed", stress_joint_fixed2::init_world),
+        (
+            "(Stress test) joint prismatic",
+            stress_joint_prismatic2::init_world,
+        ),
+    ];
+
+    // Lexicographic sort, with stress tests moved at the end of the list.
+    builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) {
+        (true, true) | (false, false) => a.0.cmp(b.0),
+        (true, false) => Ordering::Greater,
+        (false, true) => Ordering::Less,
+    });
+
+    let i = builders
+        .iter()
+        .position(|builder| builder.0.to_camel_case().as_str() == demo.as_str())
+        .unwrap_or(0);
+    let testbed = Testbed::from_builders(i, builders);
+
+    testbed.run()
+}

examples2d/balls2.rs

@@ -0,0 +1,68 @@
+use na::Point2;
+use rapier2d::dynamics::{BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+    /*
+     * Ground
+     */
+    let _ground_size = 25.0;
+
+    /*
+    let ground_shape = ShapeHandle::new(Cuboid::new(Vector2::new(ground_size, 1.0)));
+
+    let ground_handle = bodies.insert(Ground::new());
+    let co = ColliderDesc::new(ground_shape)
+        .translation(-Vector2::y())
+        .build(BodyPartHandle(ground_handle, 0));
+    colliders.insert(co);
+    */
+
+    /*
+     * Create the balls
+     */
+    let num = 50;
+    let rad = 1.0;
+
+    let shiftx = rad * 2.5;
+    let shifty = rad * 2.0;
+    let centerx = shiftx * (num as f32) / 2.0;
+    let centery = shifty / 2.0;
+
+    for i in 0..num {
+        for j in 0usize..num * 5 {
+            let x = i as f32 * shiftx - centerx;
+            let y = j as f32 * shifty + centery;
+
+            let status = if j == 0 {
+                BodyStatus::Static
+            } else {
+                BodyStatus::Dynamic
+            };
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new(status).translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad).density(1.0).build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 2.5), 5.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}

examples2d/boxes2.rs

@@ -0,0 +1,73 @@
+use na::Point2;
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 25.0;
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(ground_size, ground_size * 2.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(-ground_size, ground_size * 2.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 26;
+    let rad = 0.5;
+
+    let shift = rad * 2.0;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 2.0;
+
+    for i in 0..num {
+        for j in 0usize..num * 5 {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shift + centery + 2.0;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 50.0), 10.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}

examples2d/capsules2.rs

@@ -0,0 +1,76 @@
+use na::Point2;
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 25.0;
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(ground_size, ground_size * 4.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(-ground_size, ground_size * 4.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 26;
+    let rad = 0.5;
+
+    let shift = rad * 2.0;
+    let shifty = rad * 5.0;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 2.0;
+
+    for i in 0..num {
+        for j in 0usize..num * 5 {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shifty + centery + 3.0;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::capsule_y(rad * 1.5, rad)
+                .density(1.0)
+                .build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 50.0), 10.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}

examples2d/debug_box_ball2.rs

@@ -0,0 +1,44 @@
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let rad = 1.0;
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -rad)
+        .rotation(std::f32::consts::PI / 4.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(rad, rad).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    // Build the dynamic box rigid body.
+    let rigid_body = RigidBodyBuilder::new_dynamic()
+        .translation(0.0, 3.0 * rad)
+        .can_sleep(false)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::ball(rad).density(1.0).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    //    testbed.look_at(Point2::new(10.0, 10.0, 10.0), Point2::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples2d/heightfield2.rs

@@ -0,0 +1,72 @@
+use na::{DVector, Point2, Vector2};
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = Vector2::new(50.0, 1.0);
+    let nsubdivs = 2000;
+
+    let heights = DVector::from_fn(nsubdivs + 1, |i, _| {
+        if i == 0 || i == nsubdivs {
+            80.0
+        } else {
+            (i as f32 * ground_size.x / (nsubdivs as f32)).cos() * 2.0
+        }
+    });
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::heightfield(heights, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 26;
+    let rad = 0.5;
+
+    let shift = rad * 2.0;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+
+    for i in 0..num {
+        for j in 0usize..num * 5 {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shift + centery + 3.0;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+
+            if j % 2 == 0 {
+                let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
+                colliders.insert(collider, handle, &mut bodies);
+            } else {
+                let collider = ColliderBuilder::ball(rad).density(1.0).build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 50.0), 10.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Heightfield", init_world)]);
+    testbed.run()
+}

examples2d/joints2.rs

@@ -0,0 +1,75 @@
+use na::Point2;
+use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    /*
+     * Create the balls
+     */
+    // Build the rigid body.
+    // NOTE: a smaller radius (e.g. 0.1) breaks Box2D so
+    // in order to be able to compare rapier with Box2D,
+    // we set it to 0.4.
+    let rad = 0.4;
+    let numi = 100; // Num vertical nodes.
+    let numk = 100; // Num horizontal nodes.
+    let shift = 1.0;
+
+    let mut body_handles = Vec::new();
+
+    for k in 0..numk {
+        for i in 0..numi {
+            let fk = k as f32;
+            let fi = i as f32;
+
+            let status = if i == 0 && (k % 4 == 0 || k == numk - 1) {
+                BodyStatus::Static
+            } else {
+                BodyStatus::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(status)
+                .translation(fk * shift, -fi * shift)
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad).density(1.0).build();
+            colliders.insert(collider, child_handle, &mut bodies);
+
+            // Vertical joint.
+            if i > 0 {
+                let parent_handle = *body_handles.last().unwrap();
+                let joint = BallJoint::new(Point2::origin(), Point2::new(0.0, shift));
+                joints.insert(&mut bodies, 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(Point2::origin(), Point2::new(-shift, 0.0));
+                joints.insert(&mut bodies, parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(numk as f32 * rad, numi as f32 * -rad), 5.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}

examples2d/kinematic2.rs

@@ -0,0 +1,93 @@
+use na::Point2;
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground.
+     */
+    let ground_size = 10.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the boxes
+     */
+    let num = 6;
+    let rad = 0.2;
+
+    let shift = rad * 2.0;
+    let centerx = shift * num as f32 / 2.0;
+    let centery = shift / 2.0 + 3.04;
+
+    for i in 0usize..num {
+        for j in 0usize..num * 50 {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shift + centery;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Setup a kinematic rigid body.
+     */
+    let platform_body = RigidBodyBuilder::new_kinematic()
+        .translation(-10.0 * rad, 1.5 + 0.8)
+        .build();
+    let platform_handle = bodies.insert(platform_body);
+    let collider = ColliderBuilder::cuboid(rad * 10.0, rad)
+        .density(1.0)
+        .build();
+    colliders.insert(collider, platform_handle, &mut bodies);
+
+    /*
+     * Setup a callback to control the platform.
+     */
+    testbed.add_callback(move |bodies, _, _, _, time| {
+        let mut platform = bodies.get_mut(platform_handle).unwrap();
+        let mut next_pos = platform.position;
+
+        let dt = 0.016;
+        next_pos.translation.vector.y += (time * 5.0).sin() * dt;
+        next_pos.translation.vector.x += time.sin() * 5.0 * dt;
+
+        if next_pos.translation.vector.x >= rad * 10.0 {
+            next_pos.translation.vector.x -= dt;
+        }
+        if next_pos.translation.vector.x <= -rad * 10.0 {
+            next_pos.translation.vector.x += dt;
+        }
+
+        platform.set_next_kinematic_position(next_pos);
+    });
+
+    /*
+     * Run the simulation.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 1.0), 40.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Kinematic body", init_world)]);
+    testbed.run()
+}

examples2d/pyramid2.rs

@@ -0,0 +1,60 @@
+use na::Point2;
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 100.0;
+    let ground_thickness = 1.0;
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).build();
+    colliders.insert(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 100;
+    let rad = 0.5;
+
+    let shift = rad * 2.0;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 2.0 + ground_thickness + rad * 1.5;
+
+    for i in 0usize..num {
+        for j in i..num {
+            let fj = j as f32;
+            let fi = i as f32;
+            let x = (fi * shift / 2.0) + (fj - fi) * shift - centerx;
+            let y = fi * shift + centery;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 2.5), 5.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}

examples2d/sensor2.rs

@@ -0,0 +1,101 @@
+use na::{Point2, Point3};
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet, Proximity};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground.
+     */
+    let ground_size = 200.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height)
+        .build();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height).build();
+    colliders.insert(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create some boxes.
+     */
+    let num = 10;
+    let rad = 0.2;
+
+    let shift = rad * 2.0;
+    let centerx = shift * num as f32 / 2.0;
+
+    for i in 0usize..num {
+        let x = i as f32 * shift - centerx;
+        let y = 3.0;
+
+        // Build the rigid body.
+        let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
+        colliders.insert(collider, handle, &mut bodies);
+
+        testbed.set_body_color(handle, Point3::new(0.5, 0.5, 1.0));
+    }
+
+    /*
+     * Create a cube that will have a ball-shaped sensor attached.
+     */
+
+    // Rigid body so that the sensor can move.
+    let sensor = RigidBodyBuilder::new_dynamic()
+        .translation(0.0, 10.0)
+        .build();
+    let sensor_handle = bodies.insert(sensor);
+
+    // Solid cube attached to the sensor which
+    // other colliders can touch.
+    let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
+    colliders.insert(collider, sensor_handle, &mut bodies);
+
+    // We create a collider desc without density because we don't
+    // want it to contribute to the rigid body mass.
+    let sensor_collider = ColliderBuilder::ball(rad * 5.0).sensor(true).build();
+    colliders.insert(sensor_collider, sensor_handle, &mut bodies);
+
+    testbed.set_body_color(sensor_handle, Point3::new(0.5, 1.0, 1.0));
+
+    // Callback that will be executed on the main loop to handle proximities.
+    testbed.add_callback(move |_, colliders, events, graphics, _| {
+        while let Ok(prox) = events.proximity_events.try_recv() {
+            let color = match prox.new_status {
+                Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0),
+                Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0),
+            };
+
+            let parent_handle1 = colliders.get(prox.collider1).unwrap().parent();
+            let parent_handle2 = colliders.get(prox.collider2).unwrap().parent();
+
+            if parent_handle1 != ground_handle && parent_handle1 != sensor_handle {
+                graphics.set_body_color(parent_handle1, color);
+            }
+            if parent_handle2 != ground_handle && parent_handle2 != sensor_handle {
+                graphics.set_body_color(parent_handle2, color);
+            }
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 1.0), 100.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Sensor", init_world)]);
+    testbed.run()
+}

examples2d/stress_joint_ball2.rs

@@ -0,0 +1,72 @@
+use na::Point2;
+use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    /*
+     * Create the balls
+     */
+    // Build the rigid body.
+    let rad = 0.4;
+    let numi = 100; // Num vertical nodes.
+    let numk = 100; // Num horizontal nodes.
+    let shift = 1.0;
+
+    let mut body_handles = Vec::new();
+
+    for k in 0..numk {
+        for i in 0..numi {
+            let fk = k as f32;
+            let fi = i as f32;
+
+            let status = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 {
+                BodyStatus::Static
+            } else {
+                BodyStatus::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(status)
+                .translation(fk * shift, -fi * shift)
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad).density(1.0).build();
+            colliders.insert(collider, child_handle, &mut bodies);
+
+            // Vertical joint.
+            if i > 0 {
+                let parent_handle = *body_handles.last().unwrap();
+                let joint = BallJoint::new(Point2::origin(), Point2::new(0.0, shift));
+                joints.insert(&mut bodies, 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(Point2::origin(), Point2::new(-shift, 0.0));
+                joints.insert(&mut bodies, parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(numk as f32 * rad, numi as f32 * -rad), 5.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}

examples2d/stress_joint_fixed2.rs

@@ -0,0 +1,85 @@
+use na::{Isometry2, Point2};
+use rapier2d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    /*
+     * Create the balls
+     */
+    // Build the rigid body.
+    let rad = 0.4;
+    let num = 30; // Num vertical nodes.
+    let shift = 1.0;
+
+    let mut body_handles = Vec::new();
+
+    for xx in 0..4 {
+        let x = xx as f32 * shift * (num as f32 + 2.0);
+
+        for yy in 0..4 {
+            let y = yy as f32 * shift * (num as f32 + 4.0);
+
+            for k in 0..num {
+                for i in 0..num {
+                    let fk = k as f32;
+                    let fi = i as f32;
+
+                    let status = if k == 0 {
+                        BodyStatus::Static
+                    } else {
+                        BodyStatus::Dynamic
+                    };
+
+                    let rigid_body = RigidBodyBuilder::new(status)
+                        .translation(x + fk * shift, y - fi * shift)
+                        .build();
+                    let child_handle = bodies.insert(rigid_body);
+                    let collider = ColliderBuilder::ball(rad).density(1.0).build();
+                    colliders.insert(collider, child_handle, &mut bodies);
+
+                    // Vertical joint.
+                    if i > 0 {
+                        let parent_handle = *body_handles.last().unwrap();
+                        let joint = FixedJoint::new(
+                            Isometry2::identity(),
+                            Isometry2::translation(0.0, shift),
+                        );
+                        joints.insert(&mut bodies, 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(
+                            Isometry2::identity(),
+                            Isometry2::translation(-shift, 0.0),
+                        );
+                        joints.insert(&mut bodies, parent_handle, child_handle, joint);
+                    }
+
+                    body_handles.push(child_handle);
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(50.0, 50.0), 5.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}

examples2d/stress_joint_prismatic2.rs

@@ -0,0 +1,69 @@
+use na::{Point2, Unit, Vector2};
+use rapier2d::dynamics::{JointSet, PrismaticJoint, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    /*
+     * Create the balls
+     */
+    // Build the rigid body.
+    let rad = 0.4;
+    let num = 10;
+    let shift = 1.0;
+
+    for l in 0..25 {
+        let y = l as f32 * shift * (num as f32 + 2.0) * 2.0;
+
+        for j in 0..50 {
+            let x = j as f32 * shift * 4.0;
+
+            let ground = RigidBodyBuilder::new_static().translation(x, y).build();
+            let mut curr_parent = bodies.insert(ground);
+            let collider = ColliderBuilder::cuboid(rad, rad).build();
+            colliders.insert(collider, curr_parent, &mut bodies);
+
+            for i in 0..num {
+                let y = y - (i + 1) as f32 * shift;
+                let density = 1.0;
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+                let curr_child = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::cuboid(rad, rad).density(density).build();
+                colliders.insert(collider, curr_child, &mut bodies);
+
+                let axis = if i % 2 == 0 {
+                    Unit::new_normalize(Vector2::new(1.0, 1.0))
+                } else {
+                    Unit::new_normalize(Vector2::new(-1.0, 1.0))
+                };
+
+                let mut prism =
+                    PrismaticJoint::new(Point2::origin(), axis, Point2::new(0.0, shift), axis);
+                prism.limits_enabled = true;
+                prism.limits[0] = -1.5;
+                prism.limits[1] = 1.5;
+                joints.insert(&mut bodies, curr_parent, curr_child, prism);
+
+                curr_parent = curr_child;
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(80.0, 80.0), 15.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}

examples3d/Cargo.toml

@@ -0,0 +1,27 @@
+[package]
+name    = "nphysics-examples-3d"
+version = "0.1.0"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+edition = "2018"
+
+[features]
+parallel = [ "rapier3d/parallel", "rapier_testbed3d/parallel" ]
+simd-stable = [ "rapier3d/simd-stable" ]
+simd-nightly = [ "rapier3d/simd-nightly" ]
+other-backends = [ "rapier_testbed3d/other-backends" ]
+enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
+
+[dependencies]
+rand       = "0.7"
+Inflector  = "0.11"
+nalgebra = "0.22"
+
+[dependencies.rapier_testbed3d]
+path = "../build/rapier_testbed3d"
+
+[dependencies.rapier3d]
+path = "../build/rapier3d"
+
+[[bin]]
+name = "all_examples3"
+path = "./all_examples3.rs"

examples3d/all_examples3.rs

@@ -0,0 +1,110 @@
+#![allow(dead_code)]
+
+extern crate nalgebra as na;
+
+#[cfg(target_arch = "wasm32")]
+use wasm_bindgen::prelude::*;
+
+use inflector::Inflector;
+
+use rapier_testbed3d::Testbed;
+use std::cmp::Ordering;
+
+mod balls3;
+mod boxes3;
+mod capsules3;
+mod debug_boxes3;
+mod debug_triangle3;
+mod domino3;
+mod heightfield3;
+mod joints3;
+mod kinematic3;
+mod pyramid3;
+mod sensor3;
+mod stacks3;
+mod stress_joint_ball3;
+mod stress_joint_fixed3;
+mod stress_joint_prismatic3;
+mod stress_joint_revolute3;
+mod stress_keva3;
+mod trimesh3;
+
+fn demo_name_from_command_line() -> Option<String> {
+    let mut args = std::env::args();
+
+    while let Some(arg) = args.next() {
+        if &arg[..] == "--example" {
+            return args.next();
+        }
+    }
+
+    None
+}
+
+#[cfg(any(target_arch = "wasm32", target_arch = "asmjs"))]
+fn demo_name_from_url() -> Option<String> {
+    None
+    //    let window = stdweb::web::window();
+    //    let hash = window.location()?.search().ok()?;
+    //    if hash.len() > 0 {
+    //        Some(hash[1..].to_string())
+    //    } else {
+    //        None
+    //    }
+}
+
+#[cfg(not(any(target_arch = "wasm32", target_arch = "asmjs")))]
+fn demo_name_from_url() -> Option<String> {
+    None
+}
+
+#[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))]
+pub fn main() {
+    let demo = demo_name_from_command_line()
+        .or_else(|| demo_name_from_url())
+        .unwrap_or(String::new())
+        .to_camel_case();
+
+    let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
+        ("Balls", balls3::init_world),
+        ("Boxes", boxes3::init_world),
+        ("Capsules", capsules3::init_world),
+        ("Domino", domino3::init_world),
+        ("Heightfield", heightfield3::init_world),
+        ("Joints", joints3::init_world),
+        ("Kinematic", kinematic3::init_world),
+        ("Stacks", stacks3::init_world),
+        ("Pyramid", pyramid3::init_world),
+        ("Sensor", sensor3::init_world),
+        ("Trimesh", trimesh3::init_world),
+        ("(Debug) boxes", debug_boxes3::init_world),
+        ("(Debug) triangle", debug_triangle3::init_world),
+        ("(Stress test) joint ball", stress_joint_ball3::init_world),
+        ("(Stress test) joint fixed", stress_joint_fixed3::init_world),
+        (
+            "(Stress test) joint revolute",
+            stress_joint_revolute3::init_world,
+        ),
+        (
+            "(Stress test) joint prismatic",
+            stress_joint_prismatic3::init_world,
+        ),
+        ("(Stress test) keva tower", stress_keva3::init_world),
+    ];
+
+    // Lexicographic sort, with stress tests moved at the end of the list.
+    builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) {
+        (true, true) | (false, false) => a.0.cmp(b.0),
+        (true, false) => Ordering::Greater,
+        (false, true) => Ordering::Less,
+    });
+
+    let i = builders
+        .iter()
+        .position(|builder| builder.0.to_camel_case().as_str() == demo.as_str())
+        .unwrap_or(0);
+
+    let testbed = Testbed::from_builders(i, builders);
+
+    testbed.run()
+}

examples3d/balls3.rs

@@ -0,0 +1,29 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    let rb = RigidBodyBuilder::new_dynamic().build();
+    let co = ColliderBuilder::cuboid(0.5, 0.5, 0.5).build();
+    let h = bodies.insert(rb);
+    colliders.insert(co, h, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}

examples3d/boxes3.rs

@@ -0,0 +1,68 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + rad;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+
+    for j in 0usize..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx + offset;
+                let y = j as f32 * shift + centery + 3.0;
+                let z = k as f32 * shift - centerz + offset;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+
+        offset -= 0.05 * rad * (num as f32 - 1.0);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/capsules3.rs

@@ -0,0 +1,69 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + rad;
+    let shifty = rad * 4.0;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+
+    for j in 0usize..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx + offset;
+                let y = j as f32 * shifty + centery + 3.0;
+                let z = k as f32 * shift - centerz + offset;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::capsule_y(rad, rad).density(1.0).build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+
+        offset -= 0.05 * rad * (num as f32 - 1.0);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/debug_boxes3.rs

@@ -0,0 +1,47 @@
+use na::{Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 100.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    // Build the dynamic box rigid body.
+    let rigid_body = RigidBodyBuilder::new_dynamic()
+        .translation(1.1, 0.0, 0.0)
+        .rotation(Vector3::new(0.8, 0.2, 0.1))
+        .can_sleep(false)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).density(1.0).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/debug_triangle3.rs

@@ -0,0 +1,48 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    // Triangle ground.
+    let vtx = [
+        Point3::new(-10.0, 0.0, -10.0),
+        Point3::new(10.0, 0.0, -10.0),
+        Point3::new(0.0, 0.0, 10.0),
+    ];
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, 0.0, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::triangle(vtx[0], vtx[1], vtx[2]).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    // Dynamic box rigid body.
+    let rigid_body = RigidBodyBuilder::new_dynamic()
+        .translation(1.1, 0.01, 0.0)
+        // .rotation(Vector3::new(0.8, 0.2, 0.1))
+        .can_sleep(false)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(20.0, 0.1, 1.0).density(1.0).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/domino3.rs

@@ -0,0 +1,87 @@
+use na::{Point3, Translation3, UnitQuaternion, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 4000;
+    let width = 1.0;
+    let thickness = 0.1;
+
+    let colors = [Point3::new(0.7, 0.5, 0.9), Point3::new(0.6, 1.0, 0.6)];
+
+    let mut curr_angle = 0.0;
+    let mut curr_rad = 10.0;
+    let mut prev_angle;
+    let mut skip = 0;
+    for i in 0..num {
+        let perimeter = 2.0 * std::f32::consts::PI * curr_rad;
+        let spacing = thickness * 4.0;
+        prev_angle = curr_angle;
+        curr_angle += 2.0 * std::f32::consts::PI * spacing / perimeter;
+        let (x, z) = curr_angle.sin_cos();
+
+        // Build the rigid body.
+        let two_pi = 2.0 * std::f32::consts::PI;
+        let nudged = curr_angle % two_pi < prev_angle % two_pi;
+        let tilt = if nudged || i == num - 1 { 0.2 } else { 0.0 };
+
+        if skip == 0 {
+            let rot = UnitQuaternion::new(Vector3::y() * curr_angle);
+            let tilt = UnitQuaternion::new(rot * Vector3::z() * tilt);
+            let position =
+                Translation3::new(x * curr_rad, width * 2.0 + ground_height, z * curr_rad)
+                    * tilt
+                    * rot;
+            let rigid_body = RigidBodyBuilder::new_dynamic().position(position).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(thickness, width * 2.0, width)
+                .density(1.0)
+                .build();
+            colliders.insert(collider, handle, &mut bodies);
+            testbed.set_body_color(handle, colors[i % 2]);
+        } else {
+            skip -= 1;
+        }
+
+        if nudged {
+            skip = 5;
+        }
+
+        curr_rad += 1.5 / perimeter;
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/heightfield3.rs

@@ -0,0 +1,78 @@
+use na::{DMatrix, Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = Vector3::new(200.0, 1.0, 200.0);
+    let nsubdivs = 20;
+
+    let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
+        if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs {
+            10.0
+        } else {
+            let x = i as f32 * ground_size.x / (nsubdivs as f32);
+            let z = j as f32 * ground_size.z / (nsubdivs as f32);
+            x.sin() + z.cos()
+        }
+    });
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::heightfield(heights, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + rad;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    for j in 0usize..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery + 3.0;
+                let z = k as f32 * shift - centerz;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+
+                if j % 2 == 0 {
+                    let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                } else {
+                    let collider = ColliderBuilder::ball(rad).density(1.0).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/joints3.rs

@@ -0,0 +1,272 @@
+use na::{Isometry3, Point3, Unit, Vector3};
+use rapier3d::dynamics::{
+    BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder,
+    RigidBodySet,
+};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+fn create_prismatic_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    num: usize,
+) {
+    let rad = 0.4;
+    let shift = 1.0;
+
+    let ground = RigidBodyBuilder::new_static()
+        .translation(origin.x, origin.y, origin.z)
+        .build();
+    let mut curr_parent = bodies.insert(ground);
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    colliders.insert(collider, curr_parent, bodies);
+
+    for i in 0..num {
+        let z = origin.z + (i + 1) as f32 * shift;
+        let density = 1.0;
+        let rigid_body = RigidBodyBuilder::new_dynamic()
+            .translation(origin.x, origin.y, z)
+            .build();
+        let curr_child = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad, rad)
+            .density(density)
+            .build();
+        colliders.insert(collider, curr_child, bodies);
+
+        let axis = if i % 2 == 0 {
+            Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0))
+        } else {
+            Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0))
+        };
+
+        let z = Vector3::z();
+        let mut prism = PrismaticJoint::new(
+            Point3::origin(),
+            axis,
+            z,
+            Point3::new(0.0, 0.0, -shift),
+            axis,
+            z,
+        );
+        prism.limits_enabled = true;
+        prism.limits[0] = -2.0;
+        prism.limits[1] = 2.0;
+        joints.insert(bodies, curr_parent, curr_child, prism);
+
+        curr_parent = curr_child;
+    }
+}
+
+fn create_revolute_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    num: usize,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    let ground = RigidBodyBuilder::new_static()
+        .translation(origin.x, origin.y, 0.0)
+        .build();
+    let mut curr_parent = bodies.insert(ground);
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    colliders.insert(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 = [
+            Isometry3::translation(origin.x, origin.y, z),
+            Isometry3::translation(origin.x + shift, origin.y, z),
+            Isometry3::translation(origin.x + shift, origin.y, z + shift),
+            Isometry3::translation(origin.x, origin.y, z + shift),
+        ];
+
+        let mut handles = [curr_parent; 4];
+        for k in 0..4 {
+            let density = 1.0;
+            let rigid_body = RigidBodyBuilder::new_dynamic()
+                .position(positions[k])
+                .build();
+            handles[k] = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad, rad)
+                .density(density)
+                .build();
+            colliders.insert(collider, handles[k], bodies);
+        }
+
+        // Setup four joints.
+        let o = Point3::origin();
+        let x = Vector3::x_axis();
+        let z = Vector3::z_axis();
+
+        let revs = [
+            RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
+            RevoluteJoint::new(o, x, Point3::new(-shift, 0.0, 0.0), x),
+            RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
+            RevoluteJoint::new(o, x, Point3::new(shift, 0.0, 0.0), x),
+        ];
+
+        joints.insert(bodies, curr_parent, handles[0], revs[0]);
+        joints.insert(bodies, handles[0], handles[1], revs[1]);
+        joints.insert(bodies, handles[1], handles[2], revs[2]);
+        joints.insert(bodies, handles[2], handles[3], revs[3]);
+
+        curr_parent = handles[3];
+    }
+}
+
+fn create_fixed_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    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;
+
+            // 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) {
+                BodyStatus::Static
+            } else {
+                BodyStatus::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(status)
+                .translation(origin.x + fk * shift, origin.y, origin.z + fi * shift)
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad).density(1.0).build();
+            colliders.insert(collider, child_handle, bodies);
+
+            // Vertical joint.
+            if i > 0 {
+                let parent_handle = *body_handles.last().unwrap();
+                let joint = FixedJoint::new(
+                    Isometry3::identity(),
+                    Isometry3::translation(0.0, 0.0, -shift),
+                );
+                joints.insert(bodies, 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(
+                    Isometry3::identity(),
+                    Isometry3::translation(-shift, 0.0, 0.0),
+                );
+                joints.insert(bodies, parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+}
+
+fn create_ball_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    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) {
+                BodyStatus::Static
+            } else {
+                BodyStatus::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(status)
+                .translation(fk * shift, 0.0, fi * shift)
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad).density(1.0).build();
+            colliders.insert(collider, child_handle, bodies);
+
+            // Vertical joint.
+            if i > 0 {
+                let parent_handle = *body_handles.last().unwrap();
+                let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift));
+                joints.insert(bodies, 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(Point3::origin(), Point3::new(-shift, 0.0, 0.0));
+                joints.insert(bodies, 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 joints = JointSet::new();
+
+    create_prismatic_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(20.0, 10.0, 0.0),
+        5,
+    );
+    create_revolute_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(20.0, 0.0, 0.0),
+        3,
+    );
+    create_fixed_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(0.0, 10.0, 0.0),
+        5,
+    );
+    create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(15.0, 5.0, 42.0), Point3::new(13.0, 1.0, 1.0));
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}

examples3d/kinematic3.rs

@@ -0,0 +1,97 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground.
+     */
+    let ground_size = 10.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the boxes
+     */
+    let num = 6;
+    let rad = 0.2;
+
+    let shift = rad * 2.0;
+    let centerx = shift * num as f32 / 2.0;
+    let centery = shift / 2.0 + 3.04;
+    let centerz = shift * num as f32 / 2.0;
+
+    for i in 0usize..num {
+        for j in 0usize..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery;
+                let z = k as f32 * shift - centerz;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Setup a kinematic rigid body.
+     */
+    let platform_body = RigidBodyBuilder::new_kinematic()
+        .translation(0.0, 1.5 + 0.8, -10.0 * rad)
+        .build();
+    let platform_handle = bodies.insert(platform_body);
+    let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0)
+        .density(1.0)
+        .build();
+    colliders.insert(collider, platform_handle, &mut bodies);
+
+    /*
+     * Setup a callback to control the platform.
+     */
+    testbed.add_callback(move |bodies, _, _, _, time| {
+        let mut platform = bodies.get_mut(platform_handle).unwrap();
+        let mut next_pos = platform.position;
+
+        let dt = 0.016;
+        next_pos.translation.vector.y += (time * 5.0).sin() * dt;
+        next_pos.translation.vector.z += time.sin() * 5.0 * dt;
+
+        if next_pos.translation.vector.z >= rad * 10.0 {
+            next_pos.translation.vector.z -= dt;
+        }
+        if next_pos.translation.vector.z <= -rad * 10.0 {
+            next_pos.translation.vector.z += dt;
+        }
+
+        platform.set_next_kinematic_position(next_pos);
+    });
+
+    /*
+     * Run the simulation.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(-10.0, 5.0, -10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Kinematic body", init_world)]);
+    testbed.run()
+}

examples3d/pyramid3.rs

@@ -0,0 +1,85 @@
+use na::{Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+fn create_pyramid(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    offset: Vector3<f32>,
+    stack_height: usize,
+    half_extents: Vector3<f32>,
+) {
+    let shift = half_extents * 2.5;
+    for i in 0usize..stack_height {
+        for j in i..stack_height {
+            for k in i..stack_height {
+                let fi = i as f32;
+                let fj = j as f32;
+                let fk = k as f32;
+                let x = (fi * shift.x / 2.0) + (fk - fi) * shift.x + offset.x
+                    - stack_height as f32 * half_extents.x;
+                let y = fi * shift.y + offset.y;
+                let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z
+                    - stack_height as f32 * half_extents.z;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let rigid_body_handle = bodies.insert(rigid_body);
+
+                let collider =
+                    ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z)
+                        .density(1.0)
+                        .build();
+                colliders.insert(collider, rigid_body_handle, bodies);
+            }
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 50.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let cube_size = 1.0;
+    let hext = Vector3::repeat(cube_size);
+    let bottomy = cube_size;
+    create_pyramid(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(0.0, bottomy, 0.0),
+        24,
+        hext,
+    );
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/sensor3.rs

@@ -0,0 +1,105 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, Proximity};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground.
+     */
+    let ground_size = 200.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create some boxes.
+     */
+    let num = 10;
+    let rad = 0.2;
+
+    let shift = rad * 2.0;
+    let centerx = shift * num as f32 / 2.0;
+    let centerz = shift * num as f32 / 2.0;
+
+    for i in 0usize..num {
+        for k in 0usize..num {
+            let x = i as f32 * shift - centerx;
+            let y = 3.0;
+            let z = k as f32 * shift - centerz;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+            colliders.insert(collider, handle, &mut bodies);
+
+            testbed.set_body_color(handle, Point3::new(0.5, 0.5, 1.0));
+        }
+    }
+
+    /*
+     * Create a cube that will have a ball-shaped sensor attached.
+     */
+
+    // Rigid body so that the sensor can move.
+    let sensor = RigidBodyBuilder::new_dynamic()
+        .translation(0.0, 10.0, 0.0)
+        .build();
+    let sensor_handle = bodies.insert(sensor);
+
+    // Solid cube attached to the sensor which
+    // other colliders can touch.
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+    colliders.insert(collider, sensor_handle, &mut bodies);
+
+    // We create a collider desc without density because we don't
+    // want it to contribute to the rigid body mass.
+    let sensor_collider = ColliderBuilder::ball(rad * 5.0).sensor(true).build();
+    colliders.insert(sensor_collider, sensor_handle, &mut bodies);
+
+    testbed.set_body_color(sensor_handle, Point3::new(0.5, 1.0, 1.0));
+
+    // Callback that will be executed on the main loop to handle proximities.
+    testbed.add_callback(move |_, colliders, events, graphics, _| {
+        while let Ok(prox) = events.proximity_events.try_recv() {
+            let color = match prox.new_status {
+                Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0),
+                Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0),
+            };
+
+            let parent_handle1 = colliders.get(prox.collider1).unwrap().parent();
+            let parent_handle2 = colliders.get(prox.collider2).unwrap().parent();
+
+            if parent_handle1 != ground_handle && parent_handle1 != sensor_handle {
+                graphics.set_body_color(parent_handle1, color);
+            }
+            if parent_handle2 != ground_handle && parent_handle2 != sensor_handle {
+                graphics.set_body_color(parent_handle2, color);
+            }
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(-6.0, 4.0, -6.0), Point3::new(0.0, 1.0, 0.0));
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Sensor", init_world)]);
+    testbed.run()
+}

examples3d/stacks3.rs

@@ -0,0 +1,195 @@
+use na::{Point3, Translation3, UnitQuaternion, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+fn create_tower_circle(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    offset: Vector3<f32>,
+    stack_height: usize,
+    nsubdivs: usize,
+    half_extents: Vector3<f32>,
+) {
+    let ang_step = std::f32::consts::PI * 2.0 / nsubdivs as f32;
+    let radius = 1.3 * nsubdivs as f32 * half_extents.x / std::f32::consts::PI;
+
+    let shift = half_extents * 2.0;
+    for i in 0usize..stack_height {
+        for j in 0..nsubdivs {
+            let fj = j as f32;
+            let fi = i as f32;
+            let y = fi * shift.y;
+            let pos = Translation3::from(offset)
+                * UnitQuaternion::new(Vector3::y() * (fi / 2.0 + fj) * ang_step)
+                * Translation3::new(0.0, y, radius);
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().position(pos).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z)
+                .density(1.0)
+                .build();
+            colliders.insert(collider, handle, bodies);
+        }
+    }
+}
+
+fn create_wall(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    offset: Vector3<f32>,
+    stack_height: usize,
+    half_extents: Vector3<f32>,
+) {
+    let shift = half_extents * 2.0;
+    for i in 0usize..stack_height {
+        for j in i..stack_height {
+            let fj = j as f32;
+            let fi = i as f32;
+            let x = offset.x;
+            let y = fi * shift.y + offset.y;
+            let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z
+                - stack_height as f32 * half_extents.z;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z)
+                .density(1.0)
+                .build();
+            colliders.insert(collider, handle, bodies);
+        }
+    }
+}
+
+fn create_pyramid(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    offset: Vector3<f32>,
+    stack_height: usize,
+    half_extents: Vector3<f32>,
+) {
+    let shift = half_extents * 2.0;
+
+    for i in 0usize..stack_height {
+        for j in i..stack_height {
+            for k in i..stack_height {
+                let fi = i as f32;
+                let fj = j as f32;
+                let fk = k as f32;
+                let x = (fi * shift.x / 2.0) + (fk - fi) * shift.x + offset.x
+                    - stack_height as f32 * half_extents.x;
+                let y = fi * shift.y + offset.y;
+                let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z
+                    - stack_height as f32 * half_extents.z;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider =
+                    ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z)
+                        .density(1.0)
+                        .build();
+                colliders.insert(collider, handle, bodies);
+            }
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let cube_size = 1.0;
+    let hext = Vector3::repeat(cube_size);
+    let bottomy = cube_size * 50.0;
+    create_pyramid(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-110.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_pyramid(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-80.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_pyramid(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-50.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_pyramid(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-20.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_wall(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-2.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_wall(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(4.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_wall(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(10.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_tower_circle(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(25.0, bottomy, 0.0),
+        8,
+        24,
+        hext,
+    );
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/stress_joint_ball3.rs

@@ -0,0 +1,70 @@
+use na::Point3;
+use rapier3d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    let rad = 0.4;
+    let num = 100;
+    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) {
+                BodyStatus::Static
+            } else {
+                BodyStatus::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(status)
+                .translation(fk * shift, 0.0, fi * shift)
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad).density(1.0).build();
+            colliders.insert(collider, child_handle, &mut bodies);
+
+            // Vertical joint.
+            if i > 0 {
+                let parent_handle = *body_handles.last().unwrap();
+                let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift));
+                joints.insert(&mut bodies, 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(Point3::origin(), Point3::new(-shift, 0.0, 0.0));
+                joints.insert(&mut bodies, parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(
+        Point3::new(-110.0, -46.0, 170.0),
+        Point3::new(54.0, -38.0, 29.0),
+    );
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}

examples3d/stress_joint_fixed3.rs

@@ -0,0 +1,92 @@
+use na::{Isometry3, Point3};
+use rapier3d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    let rad = 0.4;
+    let num = 5;
+    let shift = 1.0;
+
+    let mut body_handles = Vec::new();
+
+    for m in 0..10 {
+        let z = m as f32 * shift * (num as f32 + 2.0);
+
+        for l in 0..10 {
+            let y = l as f32 * shift * 3.0;
+
+            for j in 0..5 {
+                let x = j as f32 * shift * (num as f32) * 2.0;
+
+                for k in 0..num {
+                    for i 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) {
+                            BodyStatus::Static
+                        } else {
+                            BodyStatus::Dynamic
+                        };
+
+                        let rigid_body = RigidBodyBuilder::new(status)
+                            .translation(x + fk * shift, y, z + fi * shift)
+                            .build();
+                        let child_handle = bodies.insert(rigid_body);
+                        let collider = ColliderBuilder::ball(rad).density(1.0).build();
+                        colliders.insert(collider, child_handle, &mut bodies);
+
+                        // Vertical joint.
+                        if i > 0 {
+                            let parent_handle = *body_handles.last().unwrap();
+                            let joint = FixedJoint::new(
+                                Isometry3::identity(),
+                                Isometry3::translation(0.0, 0.0, -shift),
+                            );
+                            joints.insert(&mut bodies, 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(
+                                Isometry3::identity(),
+                                Isometry3::translation(-shift, 0.0, 0.0),
+                            );
+                            joints.insert(&mut bodies, parent_handle, child_handle, joint);
+                        }
+
+                        body_handles.push(child_handle);
+                    }
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(
+        Point3::new(-38.0, 14.0, 108.0),
+        Point3::new(46.0, 12.0, 23.0),
+    );
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}

examples3d/stress_joint_prismatic3.rs

@@ -0,0 +1,81 @@
+use na::{Point3, Unit, Vector3};
+use rapier3d::dynamics::{JointSet, PrismaticJoint, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    let rad = 0.4;
+    let num = 5;
+    let shift = 1.0;
+
+    for m in 0..8 {
+        let z = m as f32 * shift * (num as f32 + 2.0);
+
+        for l in 0..8 {
+            let y = l as f32 * shift * (num as f32) * 2.0;
+
+            for j in 0..50 {
+                let x = j as f32 * shift * 4.0;
+
+                let ground = RigidBodyBuilder::new_static().translation(x, y, z).build();
+                let mut curr_parent = bodies.insert(ground);
+                let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+                colliders.insert(collider, curr_parent, &mut bodies);
+
+                for i in 0..num {
+                    let z = z + (i + 1) as f32 * shift;
+                    let density = 1.0;
+                    let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                    let curr_child = bodies.insert(rigid_body);
+                    let collider = ColliderBuilder::cuboid(rad, rad, rad)
+                        .density(density)
+                        .build();
+                    colliders.insert(collider, curr_child, &mut bodies);
+
+                    let axis = if i % 2 == 0 {
+                        Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0))
+                    } else {
+                        Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0))
+                    };
+
+                    let z = Vector3::z();
+                    let mut prism = PrismaticJoint::new(
+                        Point3::origin(),
+                        axis,
+                        z,
+                        Point3::new(0.0, 0.0, -shift),
+                        axis,
+                        z,
+                    );
+                    prism.limits_enabled = true;
+                    prism.limits[0] = -2.0;
+                    prism.limits[1] = 2.0;
+                    joints.insert(&mut bodies, curr_parent, curr_child, prism);
+
+                    curr_parent = curr_child;
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(
+        Point3::new(262.0, 63.0, 124.0),
+        Point3::new(101.0, 4.0, -3.0),
+    );
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}

examples3d/stress_joint_revolute3.rs

@@ -0,0 +1,89 @@
+use na::{Isometry3, Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RevoluteJoint, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    let rad = 0.4;
+    let num = 10;
+    let shift = 2.0;
+
+    for l in 0..4 {
+        let y = l as f32 * shift * (num as f32) * 3.0;
+
+        for j in 0..50 {
+            let x = j as f32 * shift * 4.0;
+
+            let ground = RigidBodyBuilder::new_static()
+                .translation(x, y, 0.0)
+                .build();
+            let mut curr_parent = bodies.insert(ground);
+            let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+            colliders.insert(collider, curr_parent, &mut bodies);
+
+            for i in 0..num {
+                // Create four bodies.
+                let z = i as f32 * shift * 2.0 + shift;
+                let positions = [
+                    Isometry3::translation(x, y, z),
+                    Isometry3::translation(x + shift, y, z),
+                    Isometry3::translation(x + shift, y, z + shift),
+                    Isometry3::translation(x, y, z + shift),
+                ];
+
+                let mut handles = [curr_parent; 4];
+                for k in 0..4 {
+                    let density = 1.0;
+                    let rigid_body = RigidBodyBuilder::new_dynamic()
+                        .position(positions[k])
+                        .build();
+                    handles[k] = bodies.insert(rigid_body);
+                    let collider = ColliderBuilder::cuboid(rad, rad, rad)
+                        .density(density)
+                        .build();
+                    colliders.insert(collider, handles[k], &mut bodies);
+                }
+
+                // Setup four joints.
+                let o = Point3::origin();
+                let x = Vector3::x_axis();
+                let z = Vector3::z_axis();
+
+                let revs = [
+                    RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
+                    RevoluteJoint::new(o, x, Point3::new(-shift, 0.0, 0.0), x),
+                    RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
+                    RevoluteJoint::new(o, x, Point3::new(shift, 0.0, 0.0), x),
+                ];
+
+                joints.insert(&mut bodies, curr_parent, handles[0], revs[0]);
+                joints.insert(&mut bodies, handles[0], handles[1], revs[1]);
+                joints.insert(&mut bodies, handles[1], handles[2], revs[2]);
+                joints.insert(&mut bodies, handles[2], handles[3], revs[3]);
+
+                curr_parent = handles[3];
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(
+        Point3::new(478.0, 83.0, 228.0),
+        Point3::new(134.0, 83.0, -116.0),
+    );
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}

examples3d/stress_keva3.rs

@@ -0,0 +1,148 @@
+use na::{Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn build_block(
+    testbed: &mut Testbed,
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    half_extents: Vector3<f32>,
+    shift: Vector3<f32>,
+    mut numx: usize,
+    numy: usize,
+    mut numz: usize,
+) {
+    let dimensions = [half_extents.xyz(), half_extents.zyx()];
+    let block_width = 2.0 * half_extents.z * numx as f32;
+    let block_height = 2.0 * half_extents.y * numy as f32;
+    let spacing = (half_extents.z * numx as f32 - half_extents.x) / (numz as f32 - 1.0);
+    let mut color0 = Point3::new(0.7, 0.5, 0.9);
+    let mut color1 = Point3::new(0.6, 1.0, 0.6);
+
+    for i in 0..numy {
+        std::mem::swap(&mut numx, &mut numz);
+        let dim = dimensions[i % 2];
+        let y = dim.y * i as f32 * 2.0;
+
+        for j in 0..numx {
+            let x = if i % 2 == 0 {
+                spacing * j as f32 * 2.0
+            } else {
+                dim.x * j as f32 * 2.0
+            };
+
+            for k in 0..numz {
+                let z = if i % 2 == 0 {
+                    dim.z * k as f32 * 2.0
+                } else {
+                    spacing * k as f32 * 2.0
+                };
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic()
+                    .translation(
+                        x + dim.x + shift.x,
+                        y + dim.y + shift.y,
+                        z + dim.z + shift.z,
+                    )
+                    .build();
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z)
+                    .density(1.0)
+                    .build();
+                colliders.insert(collider, handle, bodies);
+
+                testbed.set_body_color(handle, color0);
+                std::mem::swap(&mut color0, &mut color1);
+            }
+        }
+    }
+
+    // Close the top.
+    let dim = half_extents.zxy();
+
+    for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize {
+        for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize {
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic()
+                .translation(
+                    i as f32 * dim.x * 2.0 + dim.x + shift.x,
+                    dim.y + shift.y + block_height,
+                    j as f32 * dim.z * 2.0 + dim.z + shift.z,
+                )
+                .build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z)
+                .density(1.0)
+                .build();
+            colliders.insert(collider, handle, bodies);
+            testbed.set_body_color(handle, color0);
+            std::mem::swap(&mut color0, &mut color1);
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 50.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let half_extents = Vector3::new(0.02, 0.1, 0.4) / 2.0 * 10.0;
+    let mut block_height = 0.0;
+    // These should only be set to odd values otherwise
+    // the blocks won't align in the nicest way.
+    let numy = [0, 9, 13, 17, 21, 41];
+    let mut num_blocks_built = 0;
+
+    for i in (1..=5).rev() {
+        let numx = i;
+        let numy = numy[i];
+        let numz = numx * 3 + 1;
+        let block_width = numx as f32 * half_extents.z * 2.0;
+        build_block(
+            testbed,
+            &mut bodies,
+            &mut colliders,
+            half_extents,
+            Vector3::new(-block_width / 2.0, block_height, -block_width / 2.0),
+            numx,
+            numy,
+            numz,
+        );
+        block_height += numy as f32 * half_extents.y * 2.0 + half_extents.x * 2.0;
+        num_blocks_built += numx * numy * numz;
+    }
+
+    println!("Num keva blocks: {}", num_blocks_built);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/trimesh3.rs

@@ -0,0 +1,88 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.0f32;
+    let ground_height = 1.0;
+    let nsubdivs = 20;
+
+    let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs);
+    let indices = quad
+        .flat_indices()
+        .chunks(3)
+        .map(|is| Point3::new(is[0], is[2], is[1]))
+        .collect();
+    let mut vertices = quad.coords;
+
+    // ncollide generates a quad with `z` as the normal.
+    // so we switch z and y here and set a random altitude at each point.
+    for p in vertices.iter_mut() {
+        p.z = p.y;
+        p.y = (p.x.sin() + p.z.cos()) * ground_height;
+
+        if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 {
+            p.y = 10.0;
+        }
+    }
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::trimesh(vertices, indices).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + rad;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    for j in 0usize..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery + 3.0;
+                let z = k as f32 * shift - centerz;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+
+                if j % 2 == 0 {
+                    let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                } else {
+                    let collider = ColliderBuilder::ball(rad).density(1.0).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

src/counters/ccd_counters.rs

@@ -0,0 +1,49 @@
+use crate::counters::Timer;
+use std::fmt::{Display, Formatter, Result};
+
+/// Performance counters related to continuous collision detection (CCD).
+#[derive(Default, Clone, Copy)]
+pub struct CCDCounters {
+    /// The number of substeps actually performed by the CCD resolution.
+    pub num_substeps: usize,
+    /// The total time spent for TOI computation in the CCD resolution.
+    pub toi_computation_time: Timer,
+    /// The total time spent for force computation and integration in the CCD resolution.
+    pub solver_time: Timer,
+    /// The total time spent by the broad-phase in the CCD resolution.
+    pub broad_phase_time: Timer,
+    /// The total time spent by the narrow-phase in the CCD resolution.
+    pub narrow_phase_time: Timer,
+}
+
+impl CCDCounters {
+    /// Creates a new counter initialized to zero.
+    pub fn new() -> Self {
+        CCDCounters {
+            num_substeps: 0,
+            toi_computation_time: Timer::new(),
+            solver_time: Timer::new(),
+            broad_phase_time: Timer::new(),
+            narrow_phase_time: Timer::new(),
+        }
+    }
+
+    /// Resets this counter to 0.
+    pub fn reset(&mut self) {
+        self.num_substeps = 0;
+        self.toi_computation_time.reset();
+        self.solver_time.reset();
+        self.broad_phase_time.reset();
+        self.narrow_phase_time.reset();
+    }
+}
+
+impl Display for CCDCounters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Number of substeps: {}", self.num_substeps)?;
+        writeln!(f, "TOI computation time: {}", self.toi_computation_time)?;
+        writeln!(f, "Constraints solver time: {}", self.solver_time)?;
+        writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?;
+        writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time)
+    }
+}

src/counters/collision_detection_counters.rs

@@ -0,0 +1,32 @@
+use crate::counters::Timer;
+use std::fmt::{Display, Formatter, Result};
+
+/// Performance counters related to collision detection.
+#[derive(Default, Clone, Copy)]
+pub struct CollisionDetectionCounters {
+    /// Number of contact pairs detected.
+    pub ncontact_pairs: usize,
+    /// Time spent for the broad-phase of the collision detection.
+    pub broad_phase_time: Timer,
+    /// Time spent for the narrow-phase of the collision detection.
+    pub narrow_phase_time: Timer,
+}
+
+impl CollisionDetectionCounters {
+    /// Creates a new counter initialized to zero.
+    pub fn new() -> Self {
+        CollisionDetectionCounters {
+            ncontact_pairs: 0,
+            broad_phase_time: Timer::new(),
+            narrow_phase_time: Timer::new(),
+        }
+    }
+}
+
+impl Display for CollisionDetectionCounters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Number of contact pairs: {}", self.ncontact_pairs)?;
+        writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?;
+        writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time)
+    }
+}

src/counters/mod.rs

@@ -0,0 +1,225 @@
+//! Counters for benchmarking various parts of the physics engine.
+
+use std::fmt::{Display, Formatter, Result};
+
+pub use self::ccd_counters::CCDCounters;
+pub use self::collision_detection_counters::CollisionDetectionCounters;
+pub use self::solver_counters::SolverCounters;
+pub use self::stages_counters::StagesCounters;
+pub use self::timer::Timer;
+
+mod ccd_counters;
+mod collision_detection_counters;
+mod solver_counters;
+mod stages_counters;
+mod timer;
+
+/// Aggregation of all the performances counters tracked by nphysics.
+#[derive(Clone, Copy)]
+pub struct Counters {
+    /// Whether thi counter is enabled or not.
+    pub enabled: bool,
+    /// Timer for a whole timestep.
+    pub step_time: Timer,
+    /// Timer used for debugging.
+    pub custom: Timer,
+    /// Counters of every satge of one time step.
+    pub stages: StagesCounters,
+    /// Counters of the collision-detection stage.
+    pub cd: CollisionDetectionCounters,
+    /// Counters of the constraints resolution and force computation stage.
+    pub solver: SolverCounters,
+    /// Counters for the CCD resolution stage.
+    pub ccd: CCDCounters,
+}
+
+impl Counters {
+    /// Create a new set of counters initialized to wero.
+    pub fn new(enabled: bool) -> Self {
+        Counters {
+            enabled,
+            step_time: Timer::new(),
+            custom: Timer::new(),
+            stages: StagesCounters::new(),
+            cd: CollisionDetectionCounters::new(),
+            solver: SolverCounters::new(),
+            ccd: CCDCounters::new(),
+        }
+    }
+
+    /// Enable all the counters.
+    pub fn enable(&mut self) {
+        self.enabled = true;
+    }
+
+    /// Return `true` if the counters are enabled.
+    pub fn enabled(&self) -> bool {
+        self.enabled
+    }
+
+    /// Disable all the counters.
+    pub fn disable(&mut self) {
+        self.enabled = false;
+    }
+
+    /// Notify that the time-step has started.
+    pub fn step_started(&mut self) {
+        if self.enabled {
+            self.step_time.start();
+        }
+    }
+
+    /// Notfy that the time-step has finished.
+    pub fn step_completed(&mut self) {
+        if self.enabled {
+            self.step_time.pause();
+        }
+    }
+
+    /// Total time spent for one  of the physics engine.
+    pub fn step_time(&self) -> f64 {
+        self.step_time.time()
+    }
+
+    /// Notify that the custom operation has started.
+    pub fn custom_started(&mut self) {
+        if self.enabled {
+            self.custom.start();
+        }
+    }
+
+    /// Notfy that the custom operation has finished.
+    pub fn custom_completed(&mut self) {
+        if self.enabled {
+            self.custom.pause();
+        }
+    }
+
+    /// Total time of a custom event.
+    pub fn custom_time(&self) -> f64 {
+        self.custom.time()
+    }
+
+    /// Set the number of constraints generated.
+    pub fn set_nconstraints(&mut self, n: usize) {
+        self.solver.nconstraints = n;
+    }
+
+    /// Set the number of contacts generated.
+    pub fn set_ncontacts(&mut self, n: usize) {
+        self.solver.ncontacts = n;
+    }
+
+    /// Set the number of contact pairs generated.
+    pub fn set_ncontact_pairs(&mut self, n: usize) {
+        self.cd.ncontact_pairs = n;
+    }
+}
+
+macro_rules! measure_method {
+    ($started:ident, $stopped:ident, $time:ident, $info:ident. $timer:ident) => {
+        impl Counters {
+            /// Start this timer.
+            pub fn $started(&mut self) {
+                if self.enabled {
+                    self.$info.$timer.start()
+                }
+            }
+
+            /// Stop this timer.
+            pub fn $stopped(&mut self) {
+                if self.enabled {
+                    self.$info.$timer.pause()
+                }
+            }
+
+            /// Gets the time elapsed for this timer.
+            pub fn $time(&self) -> f64 {
+                if self.enabled {
+                    self.$info.$timer.time()
+                } else {
+                    0.0
+                }
+            }
+        }
+    };
+}
+
+measure_method!(
+    update_started,
+    update_completed,
+    update_time,
+    stages.update_time
+);
+measure_method!(
+    collision_detection_started,
+    collision_detection_completed,
+    collision_detection_time,
+    stages.collision_detection_time
+);
+measure_method!(
+    island_construction_started,
+    island_construction_completed,
+    island_construction_time,
+    stages.island_construction_time
+);
+measure_method!(
+    solver_started,
+    solver_completed,
+    solver_time,
+    stages.solver_time
+);
+measure_method!(ccd_started, ccd_completed, ccd_time, stages.ccd_time);
+
+measure_method!(
+    assembly_started,
+    assembly_completed,
+    assembly_time,
+    solver.velocity_assembly_time
+);
+measure_method!(
+    velocity_resolution_started,
+    velocity_resolution_completed,
+    velocity_resolution_time,
+    solver.velocity_resolution_time
+);
+measure_method!(
+    velocity_update_started,
+    velocity_update_completed,
+    velocity_update_time,
+    solver.velocity_update_time
+);
+measure_method!(
+    position_resolution_started,
+    position_resolution_completed,
+    position_resolution_time,
+    solver.position_resolution_time
+);
+measure_method!(
+    broad_phase_started,
+    broad_phase_completed,
+    broad_phase_time,
+    cd.broad_phase_time
+);
+measure_method!(
+    narrow_phase_started,
+    narrow_phase_completed,
+    narrow_phase_time,
+    cd.narrow_phase_time
+);
+
+impl Display for Counters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Total timestep time: {}", self.step_time)?;
+        self.stages.fmt(f)?;
+        self.cd.fmt(f)?;
+        self.solver.fmt(f)?;
+        writeln!(f, "Custom timer: {}", self.custom)
+    }
+}
+
+impl Default for Counters {
+    fn default() -> Self {
+        Self::new(false)
+    }
+}

src/counters/solver_counters.rs

@@ -0,0 +1,67 @@
+use crate::counters::Timer;
+use std::fmt::{Display, Formatter, Result};
+
+/// Performance counters related to constraints resolution.
+#[derive(Default, Clone, Copy)]
+pub struct SolverCounters {
+    /// Number of constraints generated.
+    pub nconstraints: usize,
+    /// Number of contacts found.
+    pub ncontacts: usize,
+    /// Time spent for the resolution of the constraints (force computation).
+    pub velocity_resolution_time: Timer,
+    /// Time spent for the assembly of all the velocity constraints.
+    pub velocity_assembly_time: Timer,
+    /// Time spent for the update of the velocity of the bodies.
+    pub velocity_update_time: Timer,
+    /// Time spent for the assembly of all the position constraints.
+    pub position_assembly_time: Timer,
+    /// Time spent for the update of the position of the bodies.
+    pub position_resolution_time: Timer,
+}
+
+impl SolverCounters {
+    /// Creates a new counter initialized to zero.
+    pub fn new() -> Self {
+        SolverCounters {
+            nconstraints: 0,
+            ncontacts: 0,
+            velocity_assembly_time: Timer::new(),
+            velocity_resolution_time: Timer::new(),
+            velocity_update_time: Timer::new(),
+            position_assembly_time: Timer::new(),
+            position_resolution_time: Timer::new(),
+        }
+    }
+
+    /// Reset all the counters to zero.
+    pub fn reset(&mut self) {
+        self.nconstraints = 0;
+        self.ncontacts = 0;
+        self.velocity_resolution_time.reset();
+        self.velocity_assembly_time.reset();
+        self.velocity_update_time.reset();
+        self.position_assembly_time.reset();
+        self.position_resolution_time.reset();
+    }
+}
+
+impl Display for SolverCounters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Number of contacts: {}", self.ncontacts)?;
+        writeln!(f, "Number of constraints: {}", self.nconstraints)?;
+        writeln!(f, "Velocity assembly time: {}", self.velocity_assembly_time)?;
+        writeln!(
+            f,
+            "Velocity resolution time: {}",
+            self.velocity_resolution_time
+        )?;
+        writeln!(f, "Velocity update time: {}", self.velocity_update_time)?;
+        writeln!(f, "Position assembly time: {}", self.position_assembly_time)?;
+        writeln!(
+            f,
+            "Position resolution time: {}",
+            self.position_resolution_time
+        )
+    }
+}

src/counters/stages_counters.rs

@@ -0,0 +1,48 @@
+use crate::counters::Timer;
+use std::fmt::{Display, Formatter, Result};
+
+/// Performance counters related to each stage of the time step.
+#[derive(Default, Clone, Copy)]
+pub struct StagesCounters {
+    /// Time spent for updating the kinematic and dynamics of every body.
+    pub update_time: Timer,
+    /// Total time spent for the collision detection (including both broad- and narrow- phases).
+    pub collision_detection_time: Timer,
+    /// Time spent for the computation of collision island and body activation/deactivation (sleeping).
+    pub island_construction_time: Timer,
+    /// Total time spent for the constraints resolution and position update.t
+    pub solver_time: Timer,
+    /// Total time spent for CCD and CCD resolution.
+    pub ccd_time: Timer,
+}
+
+impl StagesCounters {
+    /// Create a new counter intialized to zero.
+    pub fn new() -> Self {
+        StagesCounters {
+            update_time: Timer::new(),
+            collision_detection_time: Timer::new(),
+            island_construction_time: Timer::new(),
+            solver_time: Timer::new(),
+            ccd_time: Timer::new(),
+        }
+    }
+}
+
+impl Display for StagesCounters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Update time: {}", self.update_time)?;
+        writeln!(
+            f,
+            "Collision detection time: {}",
+            self.collision_detection_time
+        )?;
+        writeln!(
+            f,
+            "Island construction time: {}",
+            self.island_construction_time
+        )?;
+        writeln!(f, "Solver time: {}", self.solver_time)?;
+        writeln!(f, "CCD time: {}", self.ccd_time)
+    }
+}

src/counters/timer.rs

@@ -0,0 +1,53 @@
+use std::fmt::{Display, Error, Formatter};
+
+/// A timer.
+#[derive(Copy, Clone, Debug, Default)]
+pub struct Timer {
+    time: f64,
+    start: Option<f64>,
+}
+
+impl Timer {
+    /// Creates a new timer initialized to zero and not started.
+    pub fn new() -> Self {
+        Timer {
+            time: 0.0,
+            start: None,
+        }
+    }
+
+    /// Resets the timer to 0.
+    pub fn reset(&mut self) {
+        self.time = 0.0
+    }
+
+    /// Start the timer.
+    pub fn start(&mut self) {
+        self.time = 0.0;
+        self.start = Some(instant::now());
+    }
+
+    /// Pause the timer.
+    pub fn pause(&mut self) {
+        if let Some(start) = self.start {
+            self.time += instant::now() - start;
+        }
+        self.start = None;
+    }
+
+    /// Resume the timer.
+    pub fn resume(&mut self) {
+        self.start = Some(instant::now());
+    }
+
+    /// The measured time between the last `.start()` and `.pause()` calls.
+    pub fn time(&self) -> f64 {
+        self.time
+    }
+}
+
+impl Display for Timer {
+    fn fmt(&self, f: &mut Formatter) -> Result<(), Error> {
+        write!(f, "{}s", self.time)
+    }
+}

src/data/graph.rs

@@ -0,0 +1,830 @@
+// This is basically a stripped down version of petgraph's UnGraph.
+// - It is not generic wrt. the index type (we always use u32).
+// - It preserves associated edge iteration order after Serialization/Deserialization.
+// - It is always undirected.
+//! A stripped-down version of petgraph's UnGraph.
+
+use std::cmp::max;
+use std::ops::{Index, IndexMut};
+
+/// Node identifier.
+#[derive(Copy, Clone, Default, PartialEq, PartialOrd, Eq, Ord, Hash, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct NodeIndex(u32);
+
+impl NodeIndex {
+    #[inline]
+    pub fn new(x: u32) -> Self {
+        NodeIndex(x)
+    }
+
+    #[inline]
+    pub fn index(self) -> usize {
+        self.0 as usize
+    }
+
+    #[inline]
+    pub fn end() -> Self {
+        NodeIndex(crate::INVALID_U32)
+    }
+
+    fn _into_edge(self) -> EdgeIndex {
+        EdgeIndex(self.0)
+    }
+}
+
+impl From<u32> for NodeIndex {
+    fn from(ix: u32) -> Self {
+        NodeIndex(ix)
+    }
+}
+
+/// Edge identifier.
+#[derive(Copy, Clone, Default, PartialEq, PartialOrd, Eq, Ord, Hash, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct EdgeIndex(u32);
+
+impl EdgeIndex {
+    #[inline]
+    pub fn new(x: u32) -> Self {
+        EdgeIndex(x)
+    }
+
+    #[inline]
+    pub fn index(self) -> usize {
+        self.0 as usize
+    }
+
+    /// An invalid `EdgeIndex` used to denote absence of an edge, for example
+    /// to end an adjacency list.
+    #[inline]
+    pub fn end() -> Self {
+        EdgeIndex(crate::INVALID_U32)
+    }
+
+    fn _into_node(self) -> NodeIndex {
+        NodeIndex(self.0)
+    }
+}
+
+impl From<u32> for EdgeIndex {
+    fn from(ix: u32) -> Self {
+        EdgeIndex(ix)
+    }
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub enum Direction {
+    Outgoing = 0,
+    Incoming = 1,
+}
+
+impl Direction {
+    fn opposite(self) -> Direction {
+        match self {
+            Direction::Outgoing => Direction::Incoming,
+            Direction::Incoming => Direction::Outgoing,
+        }
+    }
+}
+
+const DIRECTIONS: [Direction; 2] = [Direction::Outgoing, Direction::Incoming];
+
+/// The graph's node type.
+#[derive(Debug, Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct Node<N> {
+    /// Associated node data.
+    pub weight: N,
+    /// Next edge in outgoing and incoming edge lists.
+    next: [EdgeIndex; 2],
+}
+
+/// The graph's edge type.
+#[derive(Debug, Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct Edge<E> {
+    /// Associated edge data.
+    pub weight: E,
+    /// Next edge in outgoing and incoming edge lists.
+    next: [EdgeIndex; 2],
+    /// Start and End node index
+    node: [NodeIndex; 2],
+}
+
+impl<E> Edge<E> {
+    /// Return the source node index.
+    pub fn source(&self) -> NodeIndex {
+        self.node[0]
+    }
+
+    /// Return the target node index.
+    pub fn target(&self) -> NodeIndex {
+        self.node[1]
+    }
+}
+
+#[derive(Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct Graph<N, E> {
+    pub(crate) nodes: Vec<Node<N>>,
+    pub(crate) edges: Vec<Edge<E>>,
+}
+
+enum Pair<T> {
+    Both(T, T),
+    One(T),
+    None,
+}
+
+/// Get mutable references at index `a` and `b`.
+fn index_twice<T>(arr: &mut [T], a: usize, b: usize) -> Pair<&mut T> {
+    if max(a, b) >= arr.len() {
+        Pair::None
+    } else if a == b {
+        Pair::One(&mut arr[max(a, b)])
+    } else {
+        // safe because a, b are in bounds and distinct
+        unsafe {
+            let ar = &mut *(arr.get_unchecked_mut(a) as *mut _);
+            let br = &mut *(arr.get_unchecked_mut(b) as *mut _);
+            Pair::Both(ar, br)
+        }
+    }
+}
+
+impl<N, E> Graph<N, E> {
+    /// Create a new `Graph` with estimated capacity.
+    pub fn with_capacity(nodes: usize, edges: usize) -> Self {
+        Graph {
+            nodes: Vec::with_capacity(nodes),
+            edges: Vec::with_capacity(edges),
+        }
+    }
+
+    /// Add a node (also called vertex) with associated data `weight` to the graph.
+    ///
+    /// Computes in **O(1)** time.
+    ///
+    /// Return the index of the new node.
+    ///
+    /// **Panics** if the Graph is at the maximum number of nodes for its index
+    /// type (N/A if usize).
+    pub fn add_node(&mut self, weight: N) -> NodeIndex {
+        let node = Node {
+            weight,
+            next: [EdgeIndex::end(), EdgeIndex::end()],
+        };
+        assert!(self.nodes.len() != crate::INVALID_USIZE);
+        let node_idx = NodeIndex::new(self.nodes.len() as u32);
+        self.nodes.push(node);
+        node_idx
+    }
+
+    /// Access the weight for node `a`.
+    ///
+    /// Also available with indexing syntax: `&graph[a]`.
+    pub fn node_weight(&self, a: NodeIndex) -> Option<&N> {
+        self.nodes.get(a.index()).map(|n| &n.weight)
+    }
+
+    /// Access the weight for edge `a`.
+    ///
+    /// Also available with indexing syntax: `&graph[a]`.
+    pub fn edge_weight(&self, a: EdgeIndex) -> Option<&E> {
+        self.edges.get(a.index()).map(|e| &e.weight)
+    }
+
+    /// Access the weight for edge `a` mutably.
+    ///
+    /// Also available with indexing syntax: `&mut graph[a]`.
+    pub fn edge_weight_mut(&mut self, a: EdgeIndex) -> Option<&mut E> {
+        self.edges.get_mut(a.index()).map(|e| &mut e.weight)
+    }
+
+    /// Add an edge from `a` to `b` to the graph, with its associated
+    /// data `weight`.
+    ///
+    /// Return the index of the new edge.
+    ///
+    /// Computes in **O(1)** time.
+    ///
+    /// **Panics** if any of the nodes don't exist.<br>
+    /// **Panics** if the Graph is at the maximum number of edges for its index
+    /// type (N/A if usize).
+    ///
+    /// **Note:** `Graph` allows adding parallel (“duplicate”) edges. If you want
+    /// to avoid this, use [`.update_edge(a, b, weight)`](#method.update_edge) instead.
+    pub fn add_edge(&mut self, a: NodeIndex, b: NodeIndex, weight: E) -> EdgeIndex {
+        assert!(self.edges.len() != crate::INVALID_USIZE);
+        let edge_idx = EdgeIndex::new(self.edges.len() as u32);
+        let mut edge = Edge {
+            weight,
+            node: [a, b],
+            next: [EdgeIndex::end(); 2],
+        };
+        match index_twice(&mut self.nodes, a.index(), b.index()) {
+            Pair::None => panic!("Graph::add_edge: node indices out of bounds"),
+            Pair::One(an) => {
+                edge.next = an.next;
+                an.next[0] = edge_idx;
+                an.next[1] = edge_idx;
+            }
+            Pair::Both(an, bn) => {
+                // a and b are different indices
+                edge.next = [an.next[0], bn.next[1]];
+                an.next[0] = edge_idx;
+                bn.next[1] = edge_idx;
+            }
+        }
+        self.edges.push(edge);
+        edge_idx
+    }
+
+    /// Access the source and target nodes for `e`.
+    pub fn edge_endpoints(&self, e: EdgeIndex) -> Option<(NodeIndex, NodeIndex)> {
+        self.edges
+            .get(e.index())
+            .map(|ed| (ed.source(), ed.target()))
+    }
+
+    /// Remove `a` from the graph if it exists, and return its weight.
+    /// If it doesn't exist in the graph, return `None`.
+    ///
+    /// Apart from `a`, this invalidates the last node index in the graph
+    /// (that node will adopt the removed node index). Edge indices are
+    /// invalidated as they would be following the removal of each edge
+    /// with an endpoint in `a`.
+    ///
+    /// Computes in **O(e')** time, where **e'** is the number of affected
+    /// edges, including *n* calls to `.remove_edge()` where *n* is the number
+    /// of edges with an endpoint in `a`, and including the edges with an
+    /// endpoint in the displaced node.
+    pub fn remove_node(&mut self, a: NodeIndex) -> Option<N> {
+        self.nodes.get(a.index())?;
+        for d in &DIRECTIONS {
+            let k = *d as usize;
+
+            // Remove all edges from and to this node.
+            loop {
+                let next = self.nodes[a.index()].next[k];
+                if next == EdgeIndex::end() {
+                    break;
+                }
+                let ret = self.remove_edge(next);
+                debug_assert!(ret.is_some());
+                let _ = ret;
+            }
+        }
+
+        // Use swap_remove -- only the swapped-in node is going to change
+        // NodeIndex, so we only have to walk its edges and update them.
+
+        let node = self.nodes.swap_remove(a.index());
+
+        // Find the edge lists of the node that had to relocate.
+        // It may be that no node had to relocate, then we are done already.
+        let swap_edges = match self.nodes.get(a.index()) {
+            None => return Some(node.weight),
+            Some(ed) => ed.next,
+        };
+
+        // The swapped element's old index
+        let old_index = NodeIndex::new(self.nodes.len() as u32);
+        let new_index = a;
+
+        // Adjust the starts of the out edges, and ends of the in edges.
+        for &d in &DIRECTIONS {
+            let k = d as usize;
+            let mut edges = edges_walker_mut(&mut self.edges, swap_edges[k], d);
+            while let Some(curedge) = edges.next_edge() {
+                debug_assert!(curedge.node[k] == old_index);
+                curedge.node[k] = new_index;
+            }
+        }
+        Some(node.weight)
+    }
+
+    /// For edge `e` with endpoints `edge_node`, replace links to it,
+    /// with links to `edge_next`.
+    fn change_edge_links(
+        &mut self,
+        edge_node: [NodeIndex; 2],
+        e: EdgeIndex,
+        edge_next: [EdgeIndex; 2],
+    ) {
+        for &d in &DIRECTIONS {
+            let k = d as usize;
+            let node = match self.nodes.get_mut(edge_node[k].index()) {
+                Some(r) => r,
+                None => {
+                    debug_assert!(
+                        false,
+                        "Edge's endpoint dir={:?} index={:?} not found",
+                        d, edge_node[k]
+                    );
+                    return;
+                }
+            };
+            let fst = node.next[k];
+            if fst == e {
+                //println!("Updating first edge 0 for node {}, set to {}", edge_node[0], edge_next[0]);
+                node.next[k] = edge_next[k];
+            } else {
+                let mut edges = edges_walker_mut(&mut self.edges, fst, d);
+                while let Some(curedge) = edges.next_edge() {
+                    if curedge.next[k] == e {
+                        curedge.next[k] = edge_next[k];
+                        break; // the edge can only be present once in the list.
+                    }
+                }
+            }
+        }
+    }
+
+    /// Remove an edge and return its edge weight, or `None` if it didn't exist.
+    ///
+    /// Apart from `e`, this invalidates the last edge index in the graph
+    /// (that edge will adopt the removed edge index).
+    ///
+    /// Computes in **O(e')** time, where **e'** is the size of four particular edge lists, for
+    /// the vertices of `e` and the vertices of another affected edge.
+    pub fn remove_edge(&mut self, e: EdgeIndex) -> Option<E> {
+        // every edge is part of two lists,
+        // outgoing and incoming edges.
+        // Remove it from both
+        let (edge_node, edge_next) = match self.edges.get(e.index()) {
+            None => return None,
+            Some(x) => (x.node, x.next),
+        };
+        // Remove the edge from its in and out lists by replacing it with
+        // a link to the next in the list.
+        self.change_edge_links(edge_node, e, edge_next);
+        self.remove_edge_adjust_indices(e)
+    }
+
+    fn remove_edge_adjust_indices(&mut self, e: EdgeIndex) -> Option<E> {
+        // swap_remove the edge -- only the removed edge
+        // and the edge swapped into place are affected and need updating
+        // indices.
+        let edge = self.edges.swap_remove(e.index());
+        let swap = match self.edges.get(e.index()) {
+            // no elment needed to be swapped.
+            None => return Some(edge.weight),
+            Some(ed) => ed.node,
+        };
+        let swapped_e = EdgeIndex::new(self.edges.len() as u32);
+
+        // Update the edge lists by replacing links to the old index by references to the new
+        // edge index.
+        self.change_edge_links(swap, swapped_e, [e, e]);
+        Some(edge.weight)
+    }
+
+    /// Return an iterator of all edges of `a`.
+    ///
+    /// - `Directed`: Outgoing edges from `a`.
+    /// - `Undirected`: All edges connected to `a`.
+    ///
+    /// Produces an empty iterator if the node doesn't exist.<br>
+    /// Iterator element type is `EdgeReference<E, Ix>`.
+    pub fn edges(&self, a: NodeIndex) -> Edges<E> {
+        self.edges_directed(a, Direction::Outgoing)
+    }
+
+    /// Return an iterator of all edges of `a`, in the specified direction.
+    ///
+    /// - `Directed`, `Outgoing`: All edges from `a`.
+    /// - `Directed`, `Incoming`: All edges to `a`.
+    /// - `Undirected`, `Outgoing`: All edges connected to `a`, with `a` being the source of each
+    ///   edge.
+    /// - `Undirected`, `Incoming`: All edges connected to `a`, with `a` being the target of each
+    ///   edge.
+    ///
+    /// Produces an empty iterator if the node `a` doesn't exist.<br>
+    /// Iterator element type is `EdgeReference<E, Ix>`.
+    pub fn edges_directed(&self, a: NodeIndex, dir: Direction) -> Edges<E> {
+        Edges {
+            skip_start: a,
+            edges: &self.edges,
+            direction: dir,
+            next: match self.nodes.get(a.index()) {
+                None => [EdgeIndex::end(), EdgeIndex::end()],
+                Some(n) => n.next,
+            },
+        }
+    }
+
+    /*
+    /// Return an iterator over all the edges connecting `a` and `b`.
+    ///
+    /// - `Directed`: Outgoing edges from `a`.
+    /// - `Undirected`: All edges connected to `a`.
+    ///
+    /// Iterator element type is `EdgeReference<E, Ix>`.
+    pub fn edges_connecting(&self, a: NodeIndex, b: NodeIndex) -> EdgesConnecting<E, Ty, Ix> {
+        EdgesConnecting {
+            target_node: b,
+            edges: self.edges_directed(a, Direction::Outgoing),
+            ty: PhantomData,
+        }
+    }
+    */
+
+    /// Lookup an edge from `a` to `b`.
+    ///
+    /// Computes in **O(e')** time, where **e'** is the number of edges
+    /// connected to `a` (and `b`, if the graph edges are undirected).
+    pub fn find_edge(&self, a: NodeIndex, b: NodeIndex) -> Option<EdgeIndex> {
+        self.find_edge_undirected(a, b).map(|(ix, _)| ix)
+    }
+
+    /// Lookup an edge between `a` and `b`, in either direction.
+    ///
+    /// If the graph is undirected, then this is equivalent to `.find_edge()`.
+    ///
+    /// Return the edge index and its directionality, with `Outgoing` meaning
+    /// from `a` to `b` and `Incoming` the reverse,
+    /// or `None` if the edge does not exist.
+    pub fn find_edge_undirected(
+        &self,
+        a: NodeIndex,
+        b: NodeIndex,
+    ) -> Option<(EdgeIndex, Direction)> {
+        match self.nodes.get(a.index()) {
+            None => None,
+            Some(node) => self.find_edge_undirected_from_node(node, b),
+        }
+    }
+
+    fn find_edge_undirected_from_node(
+        &self,
+        node: &Node<N>,
+        b: NodeIndex,
+    ) -> Option<(EdgeIndex, Direction)> {
+        for &d in &DIRECTIONS {
+            let k = d as usize;
+            let mut edix = node.next[k];
+            while let Some(edge) = self.edges.get(edix.index()) {
+                if edge.node[1 - k] == b {
+                    return Some((edix, d));
+                }
+                edix = edge.next[k];
+            }
+        }
+        None
+    }
+
+    /// Access the internal node array.
+    pub fn raw_nodes(&self) -> &[Node<N>] {
+        &self.nodes
+    }
+
+    /// Access the internal edge array.
+    pub fn raw_edges(&self) -> &[Edge<E>] {
+        &self.edges
+    }
+
+    /// Accessor for data structure internals: the first edge in the given direction.
+    pub fn first_edge(&self, a: NodeIndex, dir: Direction) -> Option<EdgeIndex> {
+        match self.nodes.get(a.index()) {
+            None => None,
+            Some(node) => {
+                let edix = node.next[dir as usize];
+                if edix == EdgeIndex::end() {
+                    None
+                } else {
+                    Some(edix)
+                }
+            }
+        }
+    }
+
+    /// Accessor for data structure internals: the next edge for the given direction.
+    pub fn next_edge(&self, e: EdgeIndex, dir: Direction) -> Option<EdgeIndex> {
+        match self.edges.get(e.index()) {
+            None => None,
+            Some(node) => {
+                let edix = node.next[dir as usize];
+                if edix == EdgeIndex::end() {
+                    None
+                } else {
+                    Some(edix)
+                }
+            }
+        }
+    }
+}
+
+/// An iterator over either the nodes without edges to them or from them.
+pub struct Externals<'a, N: 'a> {
+    iter: std::iter::Enumerate<std::slice::Iter<'a, Node<N>>>,
+    dir: Direction,
+}
+
+impl<'a, N: 'a> Iterator for Externals<'a, N> {
+    type Item = NodeIndex;
+    fn next(&mut self) -> Option<NodeIndex> {
+        let k = self.dir as usize;
+        loop {
+            match self.iter.next() {
+                None => return None,
+                Some((index, node)) => {
+                    if node.next[k] == EdgeIndex::end() && node.next[1 - k] == EdgeIndex::end() {
+                        return Some(NodeIndex::new(index as u32));
+                    } else {
+                        continue;
+                    }
+                }
+            }
+        }
+    }
+}
+
+/// Iterator over the neighbors of a node.
+///
+/// Iterator element type is `NodeIndex`.
+///
+/// Created with [`.neighbors()`][1], [`.neighbors_directed()`][2] or
+/// [`.neighbors_undirected()`][3].
+///
+/// [1]: struct.Graph.html#method.neighbors
+/// [2]: struct.Graph.html#method.neighbors_directed
+/// [3]: struct.Graph.html#method.neighbors_undirected
+pub struct Neighbors<'a, E: 'a> {
+    /// starting node to skip over
+    skip_start: NodeIndex,
+    edges: &'a [Edge<E>],
+    next: [EdgeIndex; 2],
+}
+
+impl<'a, E> Iterator for Neighbors<'a, E> {
+    type Item = NodeIndex;
+
+    fn next(&mut self) -> Option<NodeIndex> {
+        // First any outgoing edges
+        match self.edges.get(self.next[0].index()) {
+            None => {}
+            Some(edge) => {
+                self.next[0] = edge.next[0];
+                return Some(edge.node[1]);
+            }
+        }
+        // Then incoming edges
+        // For an "undirected" iterator (traverse both incoming
+        // and outgoing edge lists), make sure we don't double
+        // count selfloops by skipping them in the incoming list.
+        while let Some(edge) = self.edges.get(self.next[1].index()) {
+            self.next[1] = edge.next[1];
+            if edge.node[0] != self.skip_start {
+                return Some(edge.node[0]);
+            }
+        }
+        None
+    }
+}
+
+struct EdgesWalkerMut<'a, E: 'a> {
+    edges: &'a mut [Edge<E>],
+    next: EdgeIndex,
+    dir: Direction,
+}
+
+fn edges_walker_mut<E>(
+    edges: &mut [Edge<E>],
+    next: EdgeIndex,
+    dir: Direction,
+) -> EdgesWalkerMut<E> {
+    EdgesWalkerMut { edges, next, dir }
+}
+
+impl<'a, E> EdgesWalkerMut<'a, E> {
+    fn next_edge(&mut self) -> Option<&mut Edge<E>> {
+        self.next().map(|t| t.1)
+    }
+
+    fn next(&mut self) -> Option<(EdgeIndex, &mut Edge<E>)> {
+        let this_index = self.next;
+        let k = self.dir as usize;
+        match self.edges.get_mut(self.next.index()) {
+            None => None,
+            Some(edge) => {
+                self.next = edge.next[k];
+                Some((this_index, edge))
+            }
+        }
+    }
+}
+
+/// Iterator over the edges of from or to a node
+pub struct Edges<'a, E: 'a> {
+    /// starting node to skip over
+    skip_start: NodeIndex,
+    edges: &'a [Edge<E>],
+
+    /// Next edge to visit.
+    next: [EdgeIndex; 2],
+
+    /// For directed graphs: the direction to iterate in
+    /// For undirected graphs: the direction of edges
+    direction: Direction,
+}
+
+impl<'a, E> Iterator for Edges<'a, E> {
+    type Item = EdgeReference<'a, E>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        //      type        direction    |    iterate over    reverse
+        //                               |
+        //    Directed      Outgoing     |      outgoing        no
+        //    Directed      Incoming     |      incoming        no
+        //   Undirected     Outgoing     |        both       incoming
+        //   Undirected     Incoming     |        both       outgoing
+
+        // For iterate_over, "both" is represented as None.
+        // For reverse, "no" is represented as None.
+        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) {
+                self.next[0] = next[0];
+                return Some(EdgeReference {
+                    index: EdgeIndex(i as u32),
+                    node: if reverse == Some(Direction::Outgoing) {
+                        swap_pair(*node)
+                    } else {
+                        *node
+                    },
+                    weight,
+                });
+            }
+        }
+
+        if iterate_over.unwrap_or(Direction::Incoming) == Direction::Incoming {
+            while let Some(Edge { node, weight, next }) = self.edges.get(self.next[1].index()) {
+                let edge_index = self.next[1];
+                self.next[1] = next[1];
+                // In any of the "both" situations, self-loops would be iterated over twice.
+                // Skip them here.
+                if iterate_over.is_none() && node[0] == self.skip_start {
+                    continue;
+                }
+
+                return Some(EdgeReference {
+                    index: edge_index,
+                    node: if reverse == Some(Direction::Incoming) {
+                        swap_pair(*node)
+                    } else {
+                        *node
+                    },
+                    weight,
+                });
+            }
+        }
+
+        None
+    }
+}
+
+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 {
+        Edges {
+            skip_start: self.skip_start,
+            edges: self.edges,
+            next: self.next,
+            direction: self.direction,
+        }
+    }
+}
+
+/// Index the `Graph` by `NodeIndex` to access node weights.
+///
+/// **Panics** if the node doesn't exist.
+impl<N, E> Index<NodeIndex> for Graph<N, E> {
+    type Output = N;
+    fn index(&self, index: NodeIndex) -> &N {
+        &self.nodes[index.index()].weight
+    }
+}
+
+/// Index the `Graph` by `NodeIndex` to access node weights.
+///
+/// **Panics** if the node doesn't exist.
+impl<N, E> IndexMut<NodeIndex> for Graph<N, E> {
+    fn index_mut(&mut self, index: NodeIndex) -> &mut N {
+        &mut self.nodes[index.index()].weight
+    }
+}
+
+/// Index the `Graph` by `EdgeIndex` to access edge weights.
+///
+/// **Panics** if the edge doesn't exist.
+impl<N, E> Index<EdgeIndex> for Graph<N, E> {
+    type Output = E;
+    fn index(&self, index: EdgeIndex) -> &E {
+        &self.edges[index.index()].weight
+    }
+}
+
+/// Index the `Graph` by `EdgeIndex` to access edge weights.
+///
+/// **Panics** if the edge doesn't exist.
+impl<N, E> IndexMut<EdgeIndex> for Graph<N, E> {
+    fn index_mut(&mut self, index: EdgeIndex) -> &mut E {
+        &mut self.edges[index.index()].weight
+    }
+}
+
+/// 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:
+pub struct WalkNeighbors {
+    skip_start: NodeIndex,
+    next: [EdgeIndex; 2],
+}
+
+impl Clone for WalkNeighbors {
+    fn clone(&self) -> Self {
+        WalkNeighbors {
+            skip_start: self.skip_start,
+            next: self.next,
+        }
+    }
+}
+
+/// Reference to a `Graph` edge.
+#[derive(Debug)]
+pub struct EdgeReference<'a, E: 'a> {
+    index: EdgeIndex,
+    node: [NodeIndex; 2],
+    weight: &'a E,
+}
+
+impl<'a, E: 'a> EdgeReference<'a, E> {
+    #[inline]
+    pub fn id(&self) -> EdgeIndex {
+        self.index
+    }
+
+    #[inline]
+    pub fn weight(&self) -> &'a E {
+        self.weight
+    }
+}
+
+impl<'a, E> Clone for EdgeReference<'a, E> {
+    fn clone(&self) -> Self {
+        *self
+    }
+}
+
+impl<'a, E> Copy for EdgeReference<'a, E> {}
+
+impl<'a, E> PartialEq for EdgeReference<'a, E>
+where
+    E: PartialEq,
+{
+    fn eq(&self, rhs: &Self) -> bool {
+        self.index == rhs.index && self.weight == rhs.weight
+    }
+}
+
+/// Iterator over all nodes of a graph.
+pub struct NodeReferences<'a, N: 'a> {
+    iter: std::iter::Enumerate<std::slice::Iter<'a, Node<N>>>,
+}
+
+impl<'a, N> Iterator for NodeReferences<'a, N> {
+    type Item = (NodeIndex, &'a N);
+
+    fn next(&mut self) -> Option<Self::Item> {
+        self.iter
+            .next()
+            .map(|(i, node)| (NodeIndex::new(i as u32), &node.weight))
+    }
+
+    fn size_hint(&self) -> (usize, Option<usize>) {
+        self.iter.size_hint()
+    }
+}
+
+impl<'a, N> DoubleEndedIterator for NodeReferences<'a, N> {
+    fn next_back(&mut self) -> Option<Self::Item> {
+        self.iter
+            .next_back()
+            .map(|(i, node)| (NodeIndex::new(i as u32), &node.weight))
+    }
+}
+
+impl<'a, N> ExactSizeIterator for NodeReferences<'a, N> {}

src/data/mod.rs

@@ -0,0 +1,4 @@
+//! Data structures modified with guaranteed deterministic behavior after deserialization.
+
+pub mod arena;
+pub(crate) mod graph;

src/dynamics/integration_parameters.rs

@@ -0,0 +1,207 @@
+/// Parameters for a time-step of the physics engine.
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct IntegrationParameters {
+    /// The timestep (default: `1.0 / 60.0`)
+    dt: f32,
+    /// The inverse of `dt`.
+    inv_dt: f32,
+    //    /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`).
+    //    ///
+    //    /// This parameter is ignored if rapier is not compiled with is `parallel` feature.
+    //    /// Refer to rayon's documentation regarding how to configure the number of threads with either
+    //    /// `rayon::ThreadPoolBuilder::new().num_threads(4).build_global().unwrap()` or `ThreadPool::install`.
+    //    /// Note that using only one thread with `multithreading_enabled` set to `true` will result on a slower
+    //    /// simulation than setting `multithreading_enabled` to `false`.
+    //    pub multithreading_enabled: bool,
+    /// If `true`, the world's `step` method will stop right after resolving exactly one CCD event (default: `false`).
+    /// This allows the user to take action during a timestep, in-between two CCD events.
+    pub return_after_ccd_substep: bool,
+    /// 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: f32,
+    /// 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: f32,
+    /// 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: f32,
+    /// Contacts at points where the involved bodies have a relative
+    /// velocity smaller than this threshold wont be affected by the restitution force (default: `1.0`).
+    pub restitution_velocity_threshold: f32,
+    /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
+    pub allowed_linear_error: f32,
+    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
+    pub prediction_distance: f32,
+    /// Amount of angular drift of joint limits the engine wont
+    /// attempt to correct (default: `0.001rad`).
+    pub allowed_angular_error: f32,
+    /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`).
+    pub max_linear_correction: f32,
+    /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`).
+    pub max_angular_correction: f32,
+    /// Maximum nonlinear SOR-prox scaling parameter when the constraint
+    /// correction direction is close to the kernel of the involved multibody's
+    /// jacobian (default: `0.2`).
+    pub max_stabilization_multiplier: f32,
+    /// Maximum number of iterations performed by the velocity constraints solver (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,
+    /// Minimum number of dynamic bodies in each active island (default: `128`).
+    pub min_island_size: usize,
+    /// Maximum number of iterations performed by the position-based constraints solver for CCD steps (default: `10`).
+    ///
+    /// This should be sufficiently high so all penetration get resolved. For example, if CCD cause your
+    /// objects to stutter, that may be because the number of CCD position iterations is too low, causing
+    /// them to remain stuck in a penetration configuration for a few frames.
+    ///
+    /// The highest this number, the highest its computational cost.
+    pub max_ccd_position_iterations: usize,
+    /// Maximum number of substeps performed by the  solver (default: `1`).
+    pub max_ccd_substeps: usize,
+    /// Controls the number of Proximity::Intersecting events generated by a trigger during CCD resolution (default: `false`).
+    ///
+    /// If false, triggers will only generate one Proximity::Intersecting event during a step, even
+    /// if another colliders repeatedly enters and leaves the triggers during multiple CCD substeps.
+    ///
+    /// If true, triggers will generate as many Proximity::Intersecting and Proximity::Disjoint/Proximity::WithinMargin
+    /// events as the number of times a collider repeatedly enters and leaves the triggers during multiple CCD substeps.
+    /// This is more computationally intensive.
+    pub multiple_ccd_substep_sensor_events_enabled: bool,
+    /// Whether penetration are taken into account in CCD resolution (default: `false`).
+    ///
+    /// If this is set to `false` two penetrating colliders will not be considered to have any time of impact
+    /// while they are penetrating. This may end up allowing some tunelling, but will avoid stuttering effect
+    /// when the constraints solver fails to completely separate two colliders after a CCD contact.
+    ///
+    /// If this is set to `true`, two penetrating colliders will be considered to have a time of impact
+    /// equal to 0 until the constraints solver manages to separate them. This will prevent tunnelling
+    /// almost completely, but may introduce stuttering effects when the constraints solver fails to completely
+    /// separate two colliders after a CCD contact.
+    // FIXME: this is a very binary way of handling penetration.
+    // We should provide a more flexible solution by letting the user choose some
+    // minimal amount of movement applied to an object that get stuck.
+    pub ccd_on_penetration_enabled: bool,
+}
+
+impl IntegrationParameters {
+    /// Creates a set of integration parameters with the given values.
+    pub fn new(
+        dt: f32,
+        //        multithreading_enabled: bool,
+        erp: f32,
+        joint_erp: f32,
+        warmstart_coeff: f32,
+        restitution_velocity_threshold: f32,
+        allowed_linear_error: f32,
+        allowed_angular_error: f32,
+        max_linear_correction: f32,
+        max_angular_correction: f32,
+        prediction_distance: f32,
+        max_stabilization_multiplier: f32,
+        max_velocity_iterations: usize,
+        max_position_iterations: usize,
+        max_ccd_position_iterations: usize,
+        max_ccd_substeps: usize,
+        return_after_ccd_substep: bool,
+        multiple_ccd_substep_sensor_events_enabled: bool,
+        ccd_on_penetration_enabled: bool,
+    ) -> Self {
+        IntegrationParameters {
+            dt,
+            inv_dt: if dt == 0.0 { 0.0 } else { 1.0 / dt },
+            //            multithreading_enabled,
+            erp,
+            joint_erp,
+            warmstart_coeff,
+            restitution_velocity_threshold,
+            allowed_linear_error,
+            allowed_angular_error,
+            max_linear_correction,
+            max_angular_correction,
+            prediction_distance,
+            max_stabilization_multiplier,
+            max_velocity_iterations,
+            max_position_iterations,
+            // 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.
+            // However we don't want it to be too small and end up with
+            // tons of islands, reducing SIMD parallelism opportunities.
+            min_island_size: 128,
+            max_ccd_position_iterations,
+            max_ccd_substeps,
+            return_after_ccd_substep,
+            multiple_ccd_substep_sensor_events_enabled,
+            ccd_on_penetration_enabled,
+        }
+    }
+
+    /// The current time-stepping length.
+    #[inline(always)]
+    pub fn dt(&self) -> f32 {
+        self.dt
+    }
+
+    /// The inverse of the time-stepping length.
+    ///
+    /// This is zero if `self.dt` is zero.
+    #[inline(always)]
+    pub fn inv_dt(&self) -> f32 {
+        self.inv_dt
+    }
+
+    /// Sets the time-stepping length.
+    ///
+    /// This automatically recompute `self.inv_dt`.
+    #[inline]
+    pub fn set_dt(&mut self, dt: f32) {
+        assert!(dt >= 0.0, "The time-stepping length cannot be negative.");
+        self.dt = dt;
+        if dt == 0.0 {
+            self.inv_dt = 0.0
+        } else {
+            self.inv_dt = 1.0 / dt
+        }
+    }
+
+    /// Sets the inverse time-stepping length (i.e. the frequency).
+    ///
+    /// This automatically recompute `self.dt`.
+    #[inline]
+    pub fn set_inv_dt(&mut self, inv_dt: f32) {
+        self.inv_dt = inv_dt;
+        if inv_dt == 0.0 {
+            self.dt = 0.0
+        } else {
+            self.dt = 1.0 / inv_dt
+        }
+    }
+}
+
+impl Default for IntegrationParameters {
+    fn default() -> Self {
+        Self::new(
+            1.0 / 60.0,
+            //            true,
+            0.2,
+            0.2,
+            1.0,
+            1.0,
+            0.005,
+            0.001,
+            0.2,
+            0.2,
+            0.002,
+            0.2,
+            4,
+            1,
+            10,
+            1,
+            false,
+            false,
+            false,
+        )
+    }
+}

src/dynamics/joint/ball_joint.rs

@@ -0,0 +1,34 @@
+use crate::math::{Point, Vector};
+
+#[derive(Copy, Clone)]
+#[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<f32>,
+    /// Where the ball joint is attached on the first body, expressed in the first body local frame.
+    pub local_anchor2: Point<f32>,
+    /// The impulse applied by this joint on the first body.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    pub impulse: Vector<f32>,
+}
+
+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<f32>, local_anchor2: Point<f32>) -> Self {
+        Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
+    }
+
+    pub(crate) fn with_impulse(
+        local_anchor1: Point<f32>,
+        local_anchor2: Point<f32>,
+        impulse: Vector<f32>,
+    ) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            impulse,
+        }
+    }
+}

src/dynamics/joint/fixed_joint.rs

@@ -0,0 +1,33 @@
+use crate::math::{Isometry, SpacialVector};
+
+#[derive(Copy, Clone)]
+#[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 FixedJoint {
+    /// 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<f32>,
+    /// 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<f32>,
+    /// 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<f32>,
+}
+
+impl FixedJoint {
+    /// Creates a new fixed joint from the frames of reference of both bodies.
+    pub fn new(local_anchor1: Isometry<f32>, local_anchor2: Isometry<f32>) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            impulse: SpacialVector::zeros(),
+        }
+    }
+}

src/dynamics/joint/joint.rs

@@ -0,0 +1,112 @@
+#[cfg(feature = "dim3")]
+use crate::dynamics::RevoluteJoint;
+use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
+
+#[derive(Copy, Clone)]
+#[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),
+}
+
+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,
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => 3,
+        }
+    }
+
+    /// 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 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)
+    }
+}
+
+#[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))]
+/// 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,
+}

src/dynamics/joint/joint_set.rs

@@ -0,0 +1,218 @@
+use super::Joint;
+use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
+
+use crate::data::arena::{Arena, Index};
+use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
+
+/// The unique identifier of a joint added to the joint set.
+pub type JointHandle = Index;
+pub(crate) type JointIndex = usize;
+pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A set of joints that can be handled by a physics `World`.
+pub struct JointSet {
+    joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
+    joint_graph: InteractionGraph<Joint>,
+}
+
+impl JointSet {
+    /// Creates a new empty set of joints.
+    pub fn new() -> Self {
+        Self {
+            joint_ids: Arena::new(),
+            joint_graph: InteractionGraph::new(),
+        }
+    }
+
+    /// An always-invalid joint handle.
+    pub fn invalid_handle() -> JointHandle {
+        JointHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
+    }
+
+    /// The number of joints on this set.
+    pub fn len(&self) -> usize {
+        self.joint_graph.graph.edges.len()
+    }
+
+    /// Retrieve the joint graph where edges are joints and nodes are rigid body handles.
+    pub fn joint_graph(&self) -> &InteractionGraph<Joint> {
+        &self.joint_graph
+    }
+
+    /// Is the given joint handle valid?
+    pub fn contains(&self, handle: JointHandle) -> bool {
+        self.joint_ids.contains(handle)
+    }
+
+    /// Gets the joint with the given handle.
+    pub fn get(&self, handle: JointHandle) -> Option<&Joint> {
+        let id = self.joint_ids.get(handle)?;
+        self.joint_graph.graph.edge_weight(*id)
+    }
+
+    /// Gets the joint with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the joint at position `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the joint position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// 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: usize) -> Option<(&Joint, JointHandle)> {
+        let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
+        Some((self.joint_graph.graph.edge_weight(*id)?, handle))
+    }
+
+    /// Iterates through all the joint on this set.
+    pub fn iter(&self) -> impl Iterator<Item = &Joint> {
+        self.joint_graph.graph.edges.iter().map(|e| &e.weight)
+    }
+
+    /// Iterates mutably through all the joint on this set.
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = &mut Joint> {
+        self.joint_graph
+            .graph
+            .edges
+            .iter_mut()
+            .map(|e| &mut e.weight)
+    }
+
+    // /// The set of joints as an array.
+    // pub(crate) fn joints(&self) -> &[JointGraphEdge] {
+    //     // self.joint_graph
+    //     //     .graph
+    //     //     .edges
+    //     //     .iter_mut()
+    //     //     .map(|e| &mut e.weight)
+    // }
+
+    #[cfg(not(feature = "parallel"))]
+    pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] {
+        &mut self.joint_graph.graph.edges[..]
+    }
+
+    #[cfg(feature = "parallel")]
+    pub(crate) fn joints_vec_mut(&mut self) -> &mut Vec<JointGraphEdge> {
+        &mut self.joint_graph.graph.edges
+    }
+
+    /// Inserts a new joint into this set and retrieve its handle.
+    pub fn insert<J>(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        body1: RigidBodyHandle,
+        body2: RigidBodyHandle,
+        joint_params: J,
+    ) -> JointHandle
+    where
+        J: Into<JointParams>,
+    {
+        let handle = self.joint_ids.insert(0.into());
+        let joint = Joint {
+            body1,
+            body2,
+            handle,
+            #[cfg(feature = "parallel")]
+            constraint_index: 0,
+            #[cfg(feature = "parallel")]
+            position_constraint_index: 0,
+            params: joint_params.into(),
+        };
+
+        let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2);
+        let (rb1, rb2) = (
+            rb1.expect("Attempt to attach a joint to a non-existing body."),
+            rb2.expect("Attempt to attach a joint to a non-existing body."),
+        );
+
+        // NOTE: the body won't have a graph index if it does not
+        // have any joint attached.
+        if !InteractionGraph::<Joint>::is_graph_index_valid(rb1.joint_graph_index) {
+            rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
+        }
+
+        if !InteractionGraph::<Joint>::is_graph_index_valid(rb2.joint_graph_index) {
+            rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
+        }
+
+        let id = self
+            .joint_graph
+            .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
+
+        self.joint_ids[handle] = id;
+        handle
+    }
+
+    /// Retrieve all the 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(
+        &self,
+        bodies: &RigidBodySet,
+        out: &mut Vec<Vec<JointIndex>>,
+    ) {
+        for out_island in &mut out[..bodies.num_islands()] {
+            out_island.clear();
+        }
+
+        // FIXME: don't iterate through all the interactions.
+        for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() {
+            let joint = &edge.weight;
+            let rb1 = &bodies[joint.body1];
+            let rb2 = &bodies[joint.body2];
+
+            if (rb1.is_dynamic() || rb2.is_dynamic())
+                && (!rb1.is_dynamic() || !rb1.is_sleeping())
+                && (!rb2.is_dynamic() || !rb2.is_sleeping())
+            {
+                let island_index = if !rb1.is_dynamic() {
+                    rb2.active_island_id
+                } else {
+                    rb1.active_island_id
+                };
+
+                out[island_index].push(i);
+            }
+        }
+    }
+
+    pub(crate) fn remove_rigid_body(
+        &mut self,
+        deleted_id: RigidBodyGraphIndex,
+        bodies: &mut RigidBodySet,
+    ) {
+        if InteractionGraph::<()>::is_graph_index_valid(deleted_id) {
+            // We have to delete each joint one by one in order to:
+            // - Wake-up the attached bodies.
+            // - Update our Handle -> graph edge mapping.
+            // Delete the node.
+            let to_delete: Vec<_> = self
+                .joint_graph
+                .interactions_with(deleted_id)
+                .map(|e| (e.0, e.1, e.2.handle))
+                .collect();
+            for (h1, h2, to_delete_handle) in to_delete {
+                let to_delete_edge_id = self.joint_ids.remove(to_delete_handle).unwrap();
+                self.joint_graph.graph.remove_edge(to_delete_edge_id);
+
+                // Update the id of the edge which took the place of the deleted one.
+                if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) {
+                    self.joint_ids[j.handle] = to_delete_edge_id;
+                }
+
+                // Wake up the attached bodies.
+                bodies.wake_up(h1);
+                bodies.wake_up(h2);
+            }
+
+            if let Some(other) = self.joint_graph.remove_node(deleted_id) {
+                // One rigid-body joint graph index may have been invalidated
+                // so we need to update it.
+                if let Some(replacement) = bodies.get_mut_internal(other) {
+                    replacement.joint_graph_index = deleted_id;
+                }
+            }
+        }
+    }
+}

src/dynamics/joint/mod.rs

@@ -0,0 +1,16 @@
+pub use self::ball_joint::BallJoint;
+pub use self::fixed_joint::FixedJoint;
+pub use self::joint::{Joint, JointParams};
+pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
+pub use self::joint_set::{JointHandle, JointSet};
+pub use self::prismatic_joint::PrismaticJoint;
+#[cfg(feature = "dim3")]
+pub use self::revolute_joint::RevoluteJoint;
+
+mod ball_joint;
+mod fixed_joint;
+mod joint;
+mod joint_set;
+mod prismatic_joint;
+#[cfg(feature = "dim3")]
+mod revolute_joint;

src/dynamics/joint/prismatic_joint.rs

@@ -0,0 +1,193 @@
+use crate::math::{Isometry, Point, Vector, DIM};
+use crate::utils::WBasis;
+use na::Unit;
+#[cfg(feature = "dim2")]
+use na::Vector2;
+#[cfg(feature = "dim3")]
+use na::Vector5;
+
+#[derive(Copy, Clone)]
+#[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.
+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<f32>,
+    /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body.
+    pub local_anchor2: Point<f32>,
+    pub(crate) local_axis1: Unit<Vector<f32>>,
+    pub(crate) local_axis2: Unit<Vector<f32>>,
+    pub(crate) basis1: [Vector<f32>; DIM - 1],
+    pub(crate) basis2: [Vector<f32>; 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<f32>,
+    /// 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<f32>,
+    /// 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: [f32; 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: f32,
+    // pub motor_enabled: bool,
+    // pub target_motor_vel: f32,
+    // pub max_motor_impulse: f32,
+    // pub motor_impulse: f32,
+}
+
+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<f32>,
+        local_axis1: Unit<Vector<f32>>,
+        local_anchor2: Point<f32>,
+        local_axis2: Unit<Vector<f32>>,
+    ) -> 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: [-f32::MAX, f32::MAX],
+            limits_impulse: 0.0,
+            // motor_enabled: false,
+            // target_motor_vel: 0.0,
+            // max_motor_impulse: f32::MAX,
+            // motor_impulse: 0.0,
+        }
+    }
+
+    /// 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<f32>,
+        local_axis1: Unit<Vector<f32>>,
+        local_tangent1: Vector<f32>,
+        local_anchor2: Point<f32>,
+        local_axis2: Unit<Vector<f32>>,
+        local_tangent2: Vector<f32>,
+    ) -> Self {
+        let basis1 = if let Some(local_bitangent1) =
+            Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3)
+        {
+            [
+                local_bitangent1.into_inner(),
+                local_bitangent1.cross(&local_axis1),
+            ]
+        } 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.into_inner(),
+                local_bitangent2.cross(&local_axis2),
+            ]
+        } else {
+            local_axis2.orthonormal_basis()
+        };
+
+        Self {
+            local_anchor1,
+            local_anchor2,
+            local_axis1,
+            local_axis2,
+            basis1,
+            basis2,
+            impulse: na::zero(),
+            limits_enabled: false,
+            limits: [-f32::MAX, f32::MAX],
+            limits_impulse: 0.0,
+            // motor_enabled: false,
+            // target_motor_vel: 0.0,
+            // max_motor_impulse: f32::MAX,
+            // motor_impulse: 0.0,
+        }
+    }
+
+    /// The local axis of this joint, expressed in the local-space of the first attached body.
+    pub fn local_axis1(&self) -> Unit<Vector<f32>> {
+        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<f32>> {
+        self.local_axis2
+    }
+
+    // FIXME: precompute this?
+    #[cfg(feature = "dim2")]
+    pub(crate) fn local_frame1(&self) -> Isometry<f32> {
+        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<f32> {
+        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<f32> {
+        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<f32> {
+        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)
+    }
+}

src/dynamics/joint/revolute_joint.rs

@@ -0,0 +1,46 @@
+use crate::math::{Point, Vector};
+use crate::utils::WBasis;
+use na::{Unit, Vector5};
+
+#[derive(Copy, Clone)]
+#[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.
+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<f32>,
+    /// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body.
+    pub local_anchor2: Point<f32>,
+    /// The rotation axis of this revolute joint expressed in the local space of the first attached body.
+    pub local_axis1: Unit<Vector<f32>>,
+    /// The rotation axis of this revolute joint expressed in the local space of the second attached body.
+    pub local_axis2: Unit<Vector<f32>>,
+    /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
+    pub basis1: [Vector<f32>; 2],
+    /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
+    pub basis2: [Vector<f32>; 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<f32>,
+}
+
+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<f32>,
+        local_axis1: Unit<Vector<f32>>,
+        local_anchor2: Point<f32>,
+        local_axis2: Unit<Vector<f32>>,
+    ) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            local_axis1,
+            local_axis2,
+            basis1: local_axis1.orthonormal_basis(),
+            basis2: local_axis2.orthonormal_basis(),
+            impulse: na::zero(),
+        }
+    }
+}

src/dynamics/mass_properties.rs

@@ -0,0 +1,206 @@
+use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector};
+use crate::utils;
+use num::Zero;
+use std::ops::{Add, AddAssign};
+#[cfg(feature = "dim3")]
+use {na::Matrix3, std::ops::MulAssign};
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// The local mass properties of a rigid-body.
+pub struct MassProperties {
+    /// The center of mass of a rigid-body expressed in its local-space.
+    pub local_com: Point<f32>,
+    /// The inverse of the mass of a rigid-body.
+    ///
+    /// If this is zero, the rigid-body is assumed to have infinite mass.
+    pub inv_mass: f32,
+    /// The inverse of the principal angular inertia of the rigid-body.
+    ///
+    /// Components set to zero are assumed to be infinite along the corresponding principal axis.
+    pub inv_principal_inertia_sqrt: AngVector<f32>,
+    #[cfg(feature = "dim3")]
+    /// The principal vectors of the local angular inertia tensor of the rigid-body.
+    pub principal_inertia_local_frame: Rotation<f32>,
+}
+
+impl MassProperties {
+    #[cfg(feature = "dim2")]
+    pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self {
+        let inv_mass = utils::inv(mass);
+        let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt());
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: AngVector<f32>) -> Self {
+        Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity())
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn with_principal_inertia_frame(
+        local_com: Point<f32>,
+        mass: f32,
+        principal_inertia: AngVector<f32>,
+        principal_inertia_local_frame: Rotation<f32>,
+    ) -> Self {
+        let inv_mass = utils::inv(mass);
+        let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt()));
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+            principal_inertia_local_frame,
+        }
+    }
+
+    /// The world-space center of mass of the rigid-body.
+    pub fn world_com(&self, pos: &Isometry<f32>) -> Point<f32> {
+        pos * self.local_com
+    }
+
+    #[cfg(feature = "dim2")]
+    /// The world-space inverse angular inertia tensor of the rigid-body.
+    pub fn world_inv_inertia_sqrt(&self, _rot: &Rotation<f32>) -> AngularInertia<f32> {
+        self.inv_principal_inertia_sqrt
+    }
+
+    #[cfg(feature = "dim3")]
+    /// The world-space inverse angular inertia tensor of the rigid-body.
+    pub fn world_inv_inertia_sqrt(&self, rot: &Rotation<f32>) -> AngularInertia<f32> {
+        if !self.inv_principal_inertia_sqrt.is_zero() {
+            let mut lhs = (rot * self.principal_inertia_local_frame)
+                .to_rotation_matrix()
+                .into_inner();
+            let rhs = lhs.transpose();
+            lhs.column_mut(0)
+                .mul_assign(self.inv_principal_inertia_sqrt.x);
+            lhs.column_mut(1)
+                .mul_assign(self.inv_principal_inertia_sqrt.y);
+            lhs.column_mut(2)
+                .mul_assign(self.inv_principal_inertia_sqrt.z);
+            let inertia = lhs * rhs;
+            AngularInertia::from_sdp_matrix(inertia)
+        } else {
+            AngularInertia::zero()
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
+    pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
+        let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e));
+        self.principal_inertia_local_frame.to_rotation_matrix()
+            * Matrix3::from_diagonal(&principal_inertia)
+            * self
+                .principal_inertia_local_frame
+                .inverse()
+                .to_rotation_matrix()
+    }
+
+    #[cfg(feature = "dim2")]
+    pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector<f32>) -> f32 {
+        if self.inv_mass != 0.0 {
+            let mass = 1.0 / self.inv_mass;
+            let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt);
+            i + shift.norm_squared() * mass
+        } else {
+            0.0
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector<f32>) -> Matrix3<f32> {
+        if self.inv_mass != 0.0 {
+            let mass = 1.0 / self.inv_mass;
+            let matrix = self.reconstruct_inertia_matrix();
+            let diag = shift.norm_squared();
+            let diagm = Matrix3::from_diagonal_element(diag);
+            matrix + (diagm + shift * shift.transpose()) * mass
+        } else {
+            Matrix3::zeros()
+        }
+    }
+}
+
+impl Zero for MassProperties {
+    fn zero() -> Self {
+        Self {
+            inv_mass: 0.0,
+            inv_principal_inertia_sqrt: na::zero(),
+            #[cfg(feature = "dim3")]
+            principal_inertia_local_frame: Rotation::identity(),
+            local_com: Point::origin(),
+        }
+    }
+
+    fn is_zero(&self) -> bool {
+        *self == Self::zero()
+    }
+}
+
+impl Add<MassProperties> for MassProperties {
+    type Output = Self;
+
+    #[cfg(feature = "dim2")]
+    fn add(self, other: MassProperties) -> Self {
+        if self.is_zero() {
+            return other;
+        } else if other.is_zero() {
+            return self;
+        }
+
+        let m1 = utils::inv(self.inv_mass);
+        let m2 = utils::inv(other.inv_mass);
+        let inv_mass = utils::inv(m1 + m2);
+        let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass;
+        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
+        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
+        let inertia = i1 + i2;
+        let inv_principal_inertia_sqrt = utils::inv(inertia.sqrt());
+
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    fn add(self, other: MassProperties) -> Self {
+        if self.is_zero() {
+            return other;
+        } else if other.is_zero() {
+            return self;
+        }
+
+        let m1 = utils::inv(self.inv_mass);
+        let m2 = utils::inv(other.inv_mass);
+        let inv_mass = utils::inv(m1 + m2);
+        let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass;
+        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
+        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
+        let inertia = i1 + i2;
+        let eigen = inertia.symmetric_eigen();
+        let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors);
+        let principal_inertia = eigen.eigenvalues;
+        let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt()));
+
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+            principal_inertia_local_frame,
+        }
+    }
+}
+
+impl AddAssign<MassProperties> for MassProperties {
+    fn add_assign(&mut self, rhs: MassProperties) {
+        *self = *self + rhs
+    }
+}

src/dynamics/mass_properties_ball.rs

@@ -0,0 +1,30 @@
+use crate::dynamics::MassProperties;
+#[cfg(feature = "dim3")]
+use crate::math::Vector;
+use crate::math::{Point, PrincipalAngularInertia};
+
+impl MassProperties {
+    pub(crate) fn ball_volume_unit_angular_inertia(
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        #[cfg(feature = "dim2")]
+        {
+            let volume = std::f32::consts::PI * radius * radius;
+            let i = radius * radius / 2.0;
+            (volume, i)
+        }
+        #[cfg(feature = "dim3")]
+        {
+            let volume = std::f32::consts::PI * radius * radius * radius * 4.0 / 3.0;
+            let i = radius * radius * 2.0 / 5.0;
+
+            (volume, Vector::repeat(i))
+        }
+    }
+
+    pub(crate) fn from_ball(density: f32, radius: f32) -> Self {
+        let (vol, unit_i) = Self::ball_volume_unit_angular_inertia(radius);
+        let mass = vol * density;
+        Self::new(Point::origin(), mass, unit_i * mass)
+    }
+}

src/dynamics/mass_properties_capsule.rs

@@ -0,0 +1,60 @@
+use crate::dynamics::MassProperties;
+#[cfg(feature = "dim3")]
+use crate::geometry::Capsule;
+use crate::math::{Point, PrincipalAngularInertia, Vector};
+
+impl MassProperties {
+    fn cylinder_y_volume_unit_inertia(
+        half_height: f32,
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        #[cfg(feature = "dim2")]
+        {
+            Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
+            let sq_radius = radius * radius;
+            let sq_height = half_height * half_height * 4.0;
+            let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
+
+            let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
+            (volume, inertia)
+        }
+    }
+
+    pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
+        let half_height = (b - a).norm() / 2.0;
+        let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
+        let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius);
+        let cap_vol = cyl_vol + ball_vol;
+        let cap_mass = cap_vol * density;
+        let mut cap_unit_i = cyl_unit_i + ball_unit_i;
+        let local_com = na::center(&a, &b);
+
+        #[cfg(feature = "dim2")]
+        {
+            let h = half_height * 2.0;
+            let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
+            cap_unit_i += extra;
+            Self::new(local_com, cap_mass, cap_unit_i * cap_mass)
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let h = half_height * 2.0;
+            let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
+            cap_unit_i.x += extra;
+            cap_unit_i.z += extra;
+            let local_frame = Capsule::new(a, b, radius).rotation_wrt_y();
+            Self::with_principal_inertia_frame(
+                local_com,
+                cap_mass,
+                cap_unit_i * cap_mass,
+                local_frame,
+            )
+        }
+    }
+}

src/dynamics/mass_properties_cuboid.rs

@@ -0,0 +1,33 @@
+use crate::dynamics::MassProperties;
+use crate::math::{Point, PrincipalAngularInertia, Vector};
+
+impl MassProperties {
+    pub(crate) fn cuboid_volume_unit_inertia(
+        half_extents: Vector<f32>,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        #[cfg(feature = "dim2")]
+        {
+            let volume = half_extents.x * half_extents.y * 4.0;
+            let ix = (half_extents.x * half_extents.x) / 3.0;
+            let iy = (half_extents.y * half_extents.y) / 3.0;
+
+            (volume, ix + iy)
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let volume = half_extents.x * half_extents.y * half_extents.z * 8.0;
+            let ix = (half_extents.x * half_extents.x) / 3.0;
+            let iy = (half_extents.y * half_extents.y) / 3.0;
+            let iz = (half_extents.z * half_extents.z) / 3.0;
+
+            (volume, Vector::new(iy + iz, ix + iz, ix + iy))
+        }
+    }
+
+    pub(crate) fn from_cuboid(density: f32, half_extents: Vector<f32>) -> Self {
+        let (vol, unit_i) = Self::cuboid_volume_unit_inertia(half_extents);
+        let mass = vol * density;
+        Self::new(Point::origin(), mass, unit_i * mass)
+    }
+}

src/dynamics/mass_properties_polygon.rs

@@ -0,0 +1,144 @@
+use crate::dynamics::MassProperties;
+use crate::math::Point;
+
+impl MassProperties {
+    pub(crate) fn from_polygon(density: f32, vertices: &[Point<f32>]) -> MassProperties {
+        let (area, com) = convex_polygon_area_and_center_of_mass(vertices);
+
+        if area == 0.0 {
+            return MassProperties::new(com, 0.0, 0.0);
+        }
+
+        let mut itot = 0.0;
+        let factor = 1.0 / 6.0;
+
+        let mut iterpeek = vertices.iter().peekable();
+        let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or
+        while let Some(elem) = iterpeek.next() {
+            let area = triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement));
+
+            // algorithm adapted from Box2D
+            let e1 = *elem - com;
+            let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com;
+
+            let ex1 = e1[0];
+            let ey1 = e1[1];
+            let ex2 = e2[0];
+            let ey2 = e2[1];
+
+            let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
+            let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
+
+            let ipart = factor * (intx2 + inty2);
+
+            itot += ipart * area;
+        }
+
+        Self::new(com, area * density, itot * density)
+    }
+}
+
+fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Point<f32>]) -> (f32, Point<f32>) {
+    let geometric_center = convex_polygon
+        .iter()
+        .fold(Point::origin(), |e1, e2| e1 + e2.coords)
+        / convex_polygon.len() as f32;
+    let mut res = Point::origin();
+    let mut areasum = 0.0;
+
+    let mut iterpeek = convex_polygon.iter().peekable();
+    let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or.
+    while let Some(elem) = iterpeek.next() {
+        let (a, b, c) = (
+            elem,
+            iterpeek.peek().unwrap_or(&firstelement),
+            &geometric_center,
+        );
+        let area = triangle_area(a, b, c);
+        let center = (a.coords + b.coords + c.coords) / 3.0;
+
+        res += center * area;
+        areasum += area;
+    }
+
+    if areasum == 0.0 {
+        (areasum, geometric_center)
+    } else {
+        (areasum, res / areasum)
+    }
+}
+
+pub fn triangle_area(pa: &Point<f32>, pb: &Point<f32>, pc: &Point<f32>) -> f32 {
+    // Kahan's formula.
+    let a = na::distance(pa, pb);
+    let b = na::distance(pb, pc);
+    let c = na::distance(pc, pa);
+
+    let (c, b, a) = sort3(&a, &b, &c);
+    let a = *a;
+    let b = *b;
+    let c = *c;
+
+    let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c));
+
+    sqr.sqrt() * 0.25
+}
+
+/// Sorts a set of three values in increasing order.
+#[inline]
+pub fn sort3<'a>(a: &'a f32, b: &'a f32, c: &'a f32) -> (&'a f32, &'a f32, &'a f32) {
+    let a_b = *a > *b;
+    let a_c = *a > *c;
+    let b_c = *b > *c;
+
+    let sa;
+    let sb;
+    let sc;
+
+    // Sort the three values.
+    // FIXME: move this to the utilities?
+    if a_b {
+        // a > b
+        if a_c {
+            // a > c
+            sc = a;
+
+            if b_c {
+                // b > c
+                sa = c;
+                sb = b;
+            } else {
+                // b <= c
+                sa = b;
+                sb = c;
+            }
+        } else {
+            // a <= c
+            sa = b;
+            sb = a;
+            sc = c;
+        }
+    } else {
+        // a < b
+        if !a_c {
+            // a <= c
+            sa = a;
+
+            if b_c {
+                // b > c
+                sb = c;
+                sc = b;
+            } else {
+                sb = b;
+                sc = c;
+            }
+        } else {
+            // a > c
+            sa = c;
+            sb = a;
+            sc = b;
+        }
+    }
+
+    (sa, sb, sc)
+}

src/dynamics/mod.rs

@@ -0,0 +1,30 @@
+//! Structures related to dynamics: bodies, joints, etc.
+
+pub use self::integration_parameters::IntegrationParameters;
+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,
+};
+pub use self::mass_properties::MassProperties;
+pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
+pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodyMut, RigidBodySet};
+// #[cfg(not(feature = "parallel"))]
+pub(crate) use self::joint::JointGraphEdge;
+#[cfg(not(feature = "parallel"))]
+pub(crate) use self::solver::IslandSolver;
+#[cfg(feature = "parallel")]
+pub(crate) use self::solver::ParallelIslandSolver;
+
+mod integration_parameters;
+mod joint;
+mod mass_properties;
+mod mass_properties_ball;
+mod mass_properties_capsule;
+mod mass_properties_cuboid;
+#[cfg(feature = "dim2")]
+mod mass_properties_polygon;
+mod rigid_body;
+mod rigid_body_set;
+mod solver;

src/dynamics/rigid_body.rs

@@ -0,0 +1,441 @@
+use crate::dynamics::MassProperties;
+use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
+use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
+use crate::utils::{WCross, WDot};
+use num::Zero;
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// The status of a body, governing the way it is affected by external forces.
+pub enum BodyStatus {
+    /// A `BodyStatus::Dynamic` body can be affected by all external forces.
+    Dynamic,
+    /// A `BodyStatus::Static` body cannot be affected by external forces.
+    Static,
+    /// A `BodyStatus::Kinematic` body cannot be affected by any external forces but can be controlled
+    /// by the user at the position level while keeping realistic one-way interaction with dynamic bodies.
+    ///
+    /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body
+    /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be
+    /// modified by the user and is independent from any contact or joint it is involved in.
+    Kinematic,
+    // Semikinematic, // A kinematic that performs automatic CCD with the static environment toi avoid traversing it?
+    // Disabled,
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A rigid body.
+///
+/// To create a new rigid-body, use the `RigidBodyBuilder` structure.
+#[derive(Debug)]
+pub struct RigidBody {
+    /// The world-space position of the rigid-body.
+    pub position: Isometry<f32>,
+    pub(crate) predicted_position: Isometry<f32>,
+    /// The local mass properties of the rigid-body.
+    pub mass_properties: MassProperties,
+    /// The world-space center of mass of the rigid-body.
+    pub world_com: Point<f32>,
+    /// The square-root of the inverse angular inertia tensor of the rigid-body.
+    pub world_inv_inertia_sqrt: AngularInertia<f32>,
+    /// The linear velocity of the rigid-body.
+    pub linvel: Vector<f32>,
+    /// The angular velocity of the rigid-body.
+    pub angvel: AngVector<f32>,
+    pub(crate) linacc: Vector<f32>,
+    pub(crate) angacc: AngVector<f32>,
+    pub(crate) colliders: Vec<ColliderHandle>,
+    /// Whether or not this rigid-body is sleeping.
+    pub activation: ActivationStatus,
+    pub(crate) joint_graph_index: RigidBodyGraphIndex,
+    pub(crate) active_island_id: usize,
+    pub(crate) active_set_id: usize,
+    pub(crate) active_set_offset: usize,
+    pub(crate) active_set_timestamp: u32,
+    /// The status of the body, governing how it is affected by external forces.
+    pub body_status: BodyStatus,
+}
+
+impl Clone for RigidBody {
+    fn clone(&self) -> Self {
+        Self {
+            colliders: Vec::new(),
+            joint_graph_index: RigidBodyGraphIndex::new(crate::INVALID_U32),
+            active_island_id: crate::INVALID_USIZE,
+            active_set_id: crate::INVALID_USIZE,
+            active_set_offset: crate::INVALID_USIZE,
+            active_set_timestamp: crate::INVALID_U32,
+            ..*self
+        }
+    }
+}
+
+impl RigidBody {
+    fn new() -> Self {
+        Self {
+            position: Isometry::identity(),
+            predicted_position: Isometry::identity(),
+            mass_properties: MassProperties::zero(),
+            world_com: Point::origin(),
+            world_inv_inertia_sqrt: AngularInertia::zero(),
+            linvel: Vector::zeros(),
+            angvel: na::zero(),
+            linacc: Vector::zeros(),
+            angacc: na::zero(),
+            colliders: Vec::new(),
+            activation: ActivationStatus::new_active(),
+            joint_graph_index: InteractionGraph::<()>::invalid_graph_index(),
+            active_island_id: 0,
+            active_set_id: 0,
+            active_set_offset: 0,
+            active_set_timestamp: 0,
+            body_status: BodyStatus::Dynamic,
+        }
+    }
+
+    pub(crate) fn integrate_accelerations(&mut self, dt: f32, gravity: Vector<f32>) {
+        if self.mass_properties.inv_mass != 0.0 {
+            self.linvel += (gravity + self.linacc) * dt;
+            self.angvel += self.angacc * dt;
+
+            // Reset the accelerations.
+            self.linacc = na::zero();
+            self.angacc = na::zero();
+        }
+    }
+
+    /// The handles of colliders attached to this rigid body.
+    pub fn colliders(&self) -> &[ColliderHandle] {
+        &self.colliders[..]
+    }
+
+    /// Is this rigid body dynamic?
+    ///
+    /// A dynamic body can move freely and is affected by forces.
+    pub fn is_dynamic(&self) -> bool {
+        self.body_status == BodyStatus::Dynamic
+    }
+
+    /// Is this rigid body kinematic?
+    ///
+    /// A kinematic body can move freely but is not affected by forces.
+    pub fn is_kinematic(&self) -> bool {
+        self.body_status == BodyStatus::Kinematic
+    }
+
+    /// Is this rigid body static?
+    ///
+    /// A static body cannot move and is not affected by forces.
+    pub fn is_static(&self) -> bool {
+        self.body_status == BodyStatus::Static
+    }
+
+    /// The mass of this rigid body.
+    ///
+    /// Returns zero if this rigid body has an infinite mass.
+    pub fn mass(&self) -> f32 {
+        crate::utils::inv(self.mass_properties.inv_mass)
+    }
+
+    /// Put this rigid body to sleep.
+    ///
+    /// A sleeping body no longer moves and is no longer simulated by the physics engine unless
+    /// it is waken up. It can be woken manually with `self.wake_up` or automatically due to
+    /// external forces like contacts.
+    pub fn sleep(&mut self) {
+        self.activation.energy = 0.0;
+        self.activation.sleeping = true;
+        self.linvel = na::zero();
+        self.angvel = na::zero();
+    }
+
+    /// Wakes up this rigid body if it is sleeping.
+    pub fn wake_up(&mut self) {
+        self.activation.sleeping = false;
+
+        if self.activation.energy == 0.0 && self.is_dynamic() {
+            self.activation.energy = self.activation.threshold.abs() * 2.0;
+        }
+    }
+
+    pub(crate) fn update_energy(&mut self) {
+        let mix_factor = 0.01;
+        let new_energy = (1.0 - mix_factor) * self.activation.energy
+            + mix_factor * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel));
+        self.activation.energy = new_energy.min(self.activation.threshold.abs() * 4.0);
+    }
+
+    /// Is this rigid body sleeping?
+    pub fn is_sleeping(&self) -> bool {
+        self.activation.sleeping
+    }
+
+    fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
+        let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses.
+        let shift = Translation::from(com.coords);
+        shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
+    }
+    pub(crate) fn integrate(&mut self, dt: f32) {
+        self.position = self.integrate_velocity(dt) * self.position;
+    }
+
+    /// Sets the position of this rigid body.
+    pub fn set_position(&mut self, pos: Isometry<f32>) {
+        self.position = pos;
+
+        // TODO: update the predicted position for dynamic bodies too?
+        if self.is_static() {
+            self.predicted_position = pos;
+        }
+    }
+
+    /// If this rigid body is kinematic, sets its future position after the next timestep integration.
+    pub fn set_next_kinematic_position(&mut self, pos: Isometry<f32>) {
+        if self.is_kinematic() {
+            self.predicted_position = pos;
+        }
+    }
+
+    pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: f32) {
+        let dpos = self.predicted_position * self.position.inverse();
+        #[cfg(feature = "dim2")]
+        {
+            self.angvel = dpos.rotation.angle() * inv_dt;
+        }
+        #[cfg(feature = "dim3")]
+        {
+            self.angvel = dpos.rotation.scaled_axis() * inv_dt;
+        }
+        self.linvel = dpos.translation.vector * inv_dt;
+    }
+
+    pub(crate) fn update_predicted_position(&mut self, dt: f32) {
+        self.predicted_position = self.integrate_velocity(dt) * self.position;
+    }
+
+    pub(crate) fn update_world_mass_properties(&mut self) {
+        self.world_com = self.mass_properties.world_com(&self.position);
+        self.world_inv_inertia_sqrt = self
+            .mass_properties
+            .world_inv_inertia_sqrt(&self.position.rotation);
+    }
+
+    /*
+     * Application of forces/impulses.
+     */
+    /// Applies a force at the center-of-mass of this rigid-body.
+    pub fn apply_force(&mut self, force: Vector<f32>) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.linacc += force * self.mass_properties.inv_mass;
+        }
+    }
+
+    /// Applies an impulse at the center-of-mass of this rigid-body.
+    pub fn apply_impulse(&mut self, impulse: Vector<f32>) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.linvel += impulse * self.mass_properties.inv_mass;
+        }
+    }
+
+    /// Applies a torque at the center-of-mass of this rigid-body.
+    #[cfg(feature = "dim2")]
+    pub fn apply_torque(&mut self, torque: f32) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
+        }
+    }
+
+    /// Applies a torque at the center-of-mass of this rigid-body.
+    #[cfg(feature = "dim3")]
+    pub fn apply_torque(&mut self, torque: Vector<f32>) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
+        }
+    }
+
+    /// Applies an impulsive torque at the center-of-mass of this rigid-body.
+    #[cfg(feature = "dim2")]
+    pub fn apply_torque_impulse(&mut self, torque_impulse: f32) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.angvel +=
+                self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
+        }
+    }
+
+    /// Applies an impulsive torque at the center-of-mass of this rigid-body.
+    #[cfg(feature = "dim3")]
+    pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<f32>) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.angvel +=
+                self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
+        }
+    }
+
+    /// Applies a force at the given world-space point of this rigid-body.
+    pub fn apply_force_at_point(&mut self, force: Vector<f32>, point: Point<f32>) {
+        let torque = (point - self.world_com).gcross(force);
+        self.apply_force(force);
+        self.apply_torque(torque);
+    }
+
+    /// Applies an impulse at the given world-space point of this rigid-body.
+    pub fn apply_impulse_at_point(&mut self, impulse: Vector<f32>, point: Point<f32>) {
+        let torque_impulse = (point - self.world_com).gcross(impulse);
+        self.apply_impulse(impulse);
+        self.apply_torque_impulse(torque_impulse);
+    }
+}
+
+/// A builder for rigid-bodies.
+pub struct RigidBodyBuilder {
+    position: Isometry<f32>,
+    linvel: Vector<f32>,
+    angvel: AngVector<f32>,
+    body_status: BodyStatus,
+    can_sleep: bool,
+}
+
+impl RigidBodyBuilder {
+    /// Initialize a new builder for a rigid body which is either static, dynamic, or kinematic.
+    pub fn new(body_status: BodyStatus) -> Self {
+        Self {
+            position: Isometry::identity(),
+            linvel: Vector::zeros(),
+            angvel: na::zero(),
+            body_status,
+            can_sleep: true,
+        }
+    }
+
+    /// Initializes the builder of a new static rigid body.
+    pub fn new_static() -> Self {
+        Self::new(BodyStatus::Static)
+    }
+
+    /// Initializes the builder of a new kinematic rigid body.
+    pub fn new_kinematic() -> Self {
+        Self::new(BodyStatus::Kinematic)
+    }
+
+    /// Initializes the builder of a new dynamic rigid body.
+    pub fn new_dynamic() -> Self {
+        Self::new(BodyStatus::Dynamic)
+    }
+
+    /// Sets the initial translation of the rigid-body to be created.
+    #[cfg(feature = "dim2")]
+    pub fn translation(mut self, x: f32, y: f32) -> Self {
+        self.position.translation.x = x;
+        self.position.translation.y = y;
+        self
+    }
+
+    /// Sets the initial translation of the rigid-body to be created.
+    #[cfg(feature = "dim3")]
+    pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self {
+        self.position.translation.x = x;
+        self.position.translation.y = y;
+        self.position.translation.z = z;
+        self
+    }
+
+    /// Sets the initial orientation of the rigid-body to be created.
+    pub fn rotation(mut self, angle: AngVector<f32>) -> Self {
+        self.position.rotation = Rotation::new(angle);
+        self
+    }
+
+    /// Sets the initial position (translation and orientation) of the rigid-body to be created.
+    pub fn position(mut self, pos: Isometry<f32>) -> Self {
+        self.position = pos;
+        self
+    }
+
+    /// Sets the initial linear velocity of the rigid-body to be created.
+    #[cfg(feature = "dim2")]
+    pub fn linvel(mut self, x: f32, y: f32) -> Self {
+        self.linvel = Vector::new(x, y);
+        self
+    }
+
+    /// Sets the initial linear velocity of the rigid-body to be created.
+    #[cfg(feature = "dim3")]
+    pub fn linvel(mut self, x: f32, y: f32, z: f32) -> Self {
+        self.linvel = Vector::new(x, y, z);
+        self
+    }
+
+    /// Sets the initial angular velocity of the rigid-body to be created.
+    pub fn angvel(mut self, angvel: AngVector<f32>) -> Self {
+        self.angvel = angvel;
+        self
+    }
+
+    /// Sets whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
+    pub fn can_sleep(mut self, can_sleep: bool) -> Self {
+        self.can_sleep = can_sleep;
+        self
+    }
+
+    /// Build a new rigid-body with the parameters configured with this builder.
+    pub fn build(&self) -> RigidBody {
+        let mut rb = RigidBody::new();
+        rb.predicted_position = self.position; // FIXME: compute the correct value?
+        rb.set_position(self.position);
+        rb.linvel = self.linvel;
+        rb.angvel = self.angvel;
+        rb.body_status = self.body_status;
+
+        if !self.can_sleep {
+            rb.activation.threshold = -1.0;
+        }
+
+        rb
+    }
+}
+
+/// The activation status of a body.
+///
+/// This controls whether a body is sleeping or not.
+/// If the threshold is negative, the body never sleeps.
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ActivationStatus {
+    /// The threshold pseudo-kinetic energy bellow which the body can fall asleep.
+    pub threshold: f32,
+    /// The current pseudo-kinetic energy of the body.
+    pub energy: f32,
+    /// Is this body already sleeping?
+    pub sleeping: bool,
+}
+
+impl ActivationStatus {
+    /// The default amount of energy bellow which a body can be put to sleep by nphysics.
+    pub fn default_threshold() -> f32 {
+        0.01
+    }
+
+    /// Create a new activation status initialised with the default activation threshold and is active.
+    pub fn new_active() -> Self {
+        ActivationStatus {
+            threshold: Self::default_threshold(),
+            energy: Self::default_threshold() * 4.0,
+            sleeping: false,
+        }
+    }
+
+    /// Create a new activation status initialised with the default activation threshold and is inactive.
+    pub fn new_inactive() -> Self {
+        ActivationStatus {
+            threshold: Self::default_threshold(),
+            energy: 0.0,
+            sleeping: true,
+        }
+    }
+
+    /// Returns `true` if the body is not asleep.
+    #[inline]
+    pub fn is_active(&self) -> bool {
+        self.energy != 0.0
+    }
+}

src/dynamics/rigid_body_set.rs

@@ -0,0 +1,518 @@
+#[cfg(feature = "parallel")]
+use rayon::prelude::*;
+
+use crate::data::arena::Arena;
+use crate::dynamics::{Joint, RigidBody};
+use crate::geometry::{ColliderSet, ContactPair, InteractionGraph};
+use crossbeam::channel::{Receiver, Sender};
+use std::ops::{Deref, DerefMut, Index, IndexMut};
+
+/// A mutable reference to a rigid-body.
+pub struct RigidBodyMut<'a> {
+    rb: &'a mut RigidBody,
+    was_sleeping: bool,
+    handle: RigidBodyHandle,
+    sender: &'a Sender<RigidBodyHandle>,
+}
+
+impl<'a> RigidBodyMut<'a> {
+    fn new(
+        handle: RigidBodyHandle,
+        rb: &'a mut RigidBody,
+        sender: &'a Sender<RigidBodyHandle>,
+    ) -> Self {
+        Self {
+            was_sleeping: rb.is_sleeping(),
+            handle,
+            sender,
+            rb,
+        }
+    }
+}
+
+impl<'a> Deref for RigidBodyMut<'a> {
+    type Target = RigidBody;
+    fn deref(&self) -> &RigidBody {
+        &*self.rb
+    }
+}
+
+impl<'a> DerefMut for RigidBodyMut<'a> {
+    fn deref_mut(&mut self) -> &mut RigidBody {
+        self.rb
+    }
+}
+
+impl<'a> Drop for RigidBodyMut<'a> {
+    fn drop(&mut self) {
+        if self.was_sleeping && !self.rb.is_sleeping() {
+            self.sender.send(self.handle).unwrap();
+        }
+    }
+}
+
+/// The unique handle of a rigid body added to a `RigidBodySet`.
+pub type RigidBodyHandle = crate::data::arena::Index;
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A pair of rigid body handles.
+pub struct BodyPair {
+    /// The first rigid body handle.
+    pub body1: RigidBodyHandle,
+    /// The second rigid body handle.
+    pub body2: RigidBodyHandle,
+}
+
+impl BodyPair {
+    pub(crate) fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self {
+        BodyPair { body1, body2 }
+    }
+
+    pub(crate) fn swap(self) -> Self {
+        Self::new(self.body2, self.body1)
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A set of rigid bodies that can be handled by a physics pipeline.
+pub struct RigidBodySet {
+    // NOTE: the pub(crate) are needed by the broad phase
+    // to avoid borrowing issues. It is also needed for
+    // parallelism because the `Receiver` breaks the Sync impl.
+    // Could we avoid this?
+    pub(crate) bodies: Arena<RigidBody>,
+    pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
+    pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
+    // Set of inactive bodies which have been modified.
+    // This typically include static bodies which have been modified.
+    pub(crate) modified_inactive_set: Vec<RigidBodyHandle>,
+    pub(crate) active_islands: Vec<usize>,
+    active_set_timestamp: u32,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    can_sleep: Vec<RigidBodyHandle>, // Workspace.
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    stack: Vec<RigidBodyHandle>, // Workspace.
+    #[cfg_attr(
+        feature = "serde-serialize",
+        serde(skip, default = "crossbeam::channel::unbounded")
+    )]
+    activation_channel: (Sender<RigidBodyHandle>, Receiver<RigidBodyHandle>),
+}
+
+impl RigidBodySet {
+    /// Create a new empty set of rigid bodies.
+    pub fn new() -> Self {
+        RigidBodySet {
+            bodies: Arena::new(),
+            active_dynamic_set: Vec::new(),
+            active_kinematic_set: Vec::new(),
+            modified_inactive_set: Vec::new(),
+            active_islands: Vec::new(),
+            active_set_timestamp: 0,
+            can_sleep: Vec::new(),
+            stack: Vec::new(),
+            activation_channel: crossbeam::channel::unbounded(),
+        }
+    }
+
+    /// An always-invalid rigid-body handle.
+    pub fn invalid_handle() -> RigidBodyHandle {
+        RigidBodyHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
+    }
+
+    /// The number of rigid bodies on this set.
+    pub fn len(&self) -> usize {
+        self.bodies.len()
+    }
+
+    pub(crate) fn activate(&mut self, handle: RigidBodyHandle) {
+        let mut rb = &mut self.bodies[handle];
+        if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
+            rb.active_set_id = self.active_dynamic_set.len();
+            self.active_dynamic_set.push(handle);
+        }
+    }
+
+    /// Is the given body handle valid?
+    pub fn contains(&self, handle: RigidBodyHandle) -> bool {
+        self.bodies.contains(handle)
+    }
+
+    /// Insert a rigid body into this set and retrieve its handle.
+    pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle {
+        let handle = self.bodies.insert(rb);
+        let rb = &mut self.bodies[handle];
+        rb.active_set_id = self.active_dynamic_set.len();
+
+        if !rb.is_sleeping() && rb.is_dynamic() {
+            self.active_dynamic_set.push(handle);
+        }
+
+        if rb.is_kinematic() {
+            self.active_kinematic_set.push(handle);
+        }
+
+        if !rb.is_dynamic() {
+            self.modified_inactive_set.push(handle);
+        }
+
+        handle
+    }
+
+    pub(crate) fn num_islands(&self) -> usize {
+        self.active_islands.len() - 1
+    }
+
+    pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option<RigidBody> {
+        let rb = self.bodies.remove(handle)?;
+        // Remove this body from the active dynamic set.
+        if self.active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
+            self.active_dynamic_set.swap_remove(rb.active_set_id);
+
+            if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) {
+                self.bodies[*replacement].active_set_id = rb.active_set_id;
+            }
+        }
+
+        Some(rb)
+    }
+
+    /// Forces the specified rigid-body to wake up if it is dynamic.
+    pub fn wake_up(&mut self, handle: RigidBodyHandle) {
+        if let Some(rb) = self.bodies.get_mut(handle) {
+            if rb.is_dynamic() {
+                rb.wake_up();
+
+                if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
+                    rb.active_set_id = self.active_dynamic_set.len();
+                    self.active_dynamic_set.push(handle);
+                }
+            }
+        }
+    }
+
+    /// Gets the rigid-body with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the rigid-body at position `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the rigid-body position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// 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: usize) -> Option<(&RigidBody, RigidBodyHandle)> {
+        self.bodies.get_unknown_gen(i)
+    }
+
+    /// Gets a mutable reference to the rigid-body with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the rigid-body at position `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the rigid-body position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// 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: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
+        self.bodies.get_unknown_gen_mut(i)
+    }
+
+    /// Gets the rigid-body with the given handle.
+    pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> {
+        self.bodies.get(handle)
+    }
+
+    /// Gets a mutable reference to the rigid-body with the given handle.
+    pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<RigidBodyMut> {
+        let sender = &self.activation_channel.0;
+        self.bodies
+            .get_mut(handle)
+            .map(|rb| RigidBodyMut::new(handle, rb, sender))
+    }
+
+    pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
+        self.bodies.get_mut(handle)
+    }
+
+    pub(crate) fn get2_mut_internal(
+        &mut self,
+        h1: RigidBodyHandle,
+        h2: RigidBodyHandle,
+    ) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
+        self.bodies.get2_mut(h1, h2)
+    }
+
+    /// Iterates through all the rigid-bodies on this set.
+    pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
+        self.bodies.iter()
+    }
+
+    /// Iterates mutably through all the rigid-bodies on this set.
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyMut)> {
+        let sender = &self.activation_channel.0;
+        self.bodies
+            .iter_mut()
+            .map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender)))
+    }
+
+    /// Iter through all the active dynamic rigid-bodies on this set.
+    pub fn iter_active_dynamic<'a>(
+        &'a self,
+    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
+        let bodies: &'a _ = &self.bodies;
+        self.active_dynamic_set
+            .iter()
+            .filter_map(move |h| Some((*h, bodies.get(*h)?)))
+    }
+
+    #[cfg(not(feature = "parallel"))]
+    pub(crate) fn iter_active_island<'a>(
+        &'a self,
+        island_id: usize,
+    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
+        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
+        let bodies: &'a _ = &self.bodies;
+        self.active_dynamic_set[island_range]
+            .iter()
+            .filter_map(move |h| Some((*h, bodies.get(*h)?)))
+    }
+
+    #[inline(always)]
+    pub(crate) fn foreach_active_body_mut_internal(
+        &mut self,
+        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
+    ) {
+        for handle in &self.active_dynamic_set {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+
+        for handle in &self.active_kinematic_set {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+    }
+
+    #[inline(always)]
+    pub(crate) fn foreach_active_dynamic_body_mut_internal(
+        &mut self,
+        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
+    ) {
+        for handle in &self.active_dynamic_set {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+    }
+
+    #[inline(always)]
+    pub(crate) fn foreach_active_kinematic_body_mut_internal(
+        &mut self,
+        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
+    ) {
+        for handle in &self.active_kinematic_set {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+    }
+
+    #[inline(always)]
+    #[cfg(not(feature = "parallel"))]
+    pub(crate) fn foreach_active_island_body_mut_internal(
+        &mut self,
+        island_id: usize,
+        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
+    ) {
+        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
+        for handle in &self.active_dynamic_set[island_range] {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+    }
+
+    #[cfg(feature = "parallel")]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub(crate) fn foreach_active_island_body_mut_internal_parallel(
+        &mut self,
+        island_id: usize,
+        f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
+    ) {
+        use std::sync::atomic::Ordering;
+
+        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
+        let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _);
+        self.active_dynamic_set[island_range]
+            .par_iter()
+            .for_each_init(
+                || bodies.load(Ordering::Relaxed),
+                |bodies, handle| {
+                    let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) };
+                    if let Some(rb) = bodies.get_mut(*handle) {
+                        f(*handle, rb)
+                    }
+                },
+            );
+    }
+
+    // pub(crate) fn active_dynamic_set(&self) -> &[RigidBodyHandle] {
+    //     &self.active_dynamic_set
+    // }
+
+    pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
+        self.active_islands[island_id]..self.active_islands[island_id + 1]
+    }
+
+    pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
+        &self.active_dynamic_set[self.active_island_range(island_id)]
+    }
+
+    pub(crate) fn maintain_active_set(&mut self) {
+        for handle in self.activation_channel.1.try_iter() {
+            if let Some(rb) = self.bodies.get_mut(handle) {
+                // Push the body to the active set if it is not
+                // sleeping and if it is not already inside of the active set.
+                if !rb.is_sleeping() // May happen if the body was put to sleep manually.
+                    && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
+                    && self.active_dynamic_set.get(rb.active_set_id) != Some(&handle)
+                {
+                    rb.active_set_id = self.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
+                    self.active_dynamic_set.push(handle);
+                }
+            }
+        }
+    }
+
+    pub(crate) fn update_active_set_with_contacts(
+        &mut self,
+        colliders: &ColliderSet,
+        contact_graph: &InteractionGraph<ContactPair>,
+        joint_graph: &InteractionGraph<Joint>,
+        min_island_size: usize,
+    ) {
+        assert!(
+            min_island_size > 0,
+            "The minimum island size must be at least 1."
+        );
+
+        // Update the energy of every rigid body and
+        // keep only those that may not sleep.
+        //        let t = instant::now();
+        self.active_set_timestamp += 1;
+        self.stack.clear();
+        self.can_sleep.clear();
+
+        // NOTE: the `.rev()` is here so that two successive timesteps preserve
+        // the order of the bodies in the `active_dynamic_set` vec. This reversal
+        // does not seem to affect performances nor stability. However it makes
+        // debugging slightly nicer so we keep this rev.
+        for h in self.active_dynamic_set.drain(..).rev() {
+            let rb = &mut self.bodies[h];
+            rb.update_energy();
+            if rb.activation.energy <= rb.activation.threshold {
+                // Mark them as sleeping for now. This will
+                // be set to false during the graph traversal
+                // if it should not be put to sleep.
+                rb.activation.sleeping = true;
+                self.can_sleep.push(h);
+            } else {
+                self.stack.push(h);
+            }
+        }
+
+        //        println!("Selection: {}", instant::now() - t);
+
+        //        let t = instant::now();
+        // Propagation of awake state and awake island computation through the
+        // traversal of the interaction graph.
+        self.active_islands.clear();
+        self.active_islands.push(0);
+
+        // The max avoid underflow when the stack is empty.
+        let mut island_marker = self.stack.len().max(1) - 1;
+
+        while let Some(handle) = self.stack.pop() {
+            let rb = &mut self.bodies[handle];
+
+            if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
+                // We already visited this body and its neighbors.
+                // Also, we don't propagate awake state through static bodies.
+                continue;
+            } else if self.stack.len() < island_marker {
+                if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
+                    >= min_island_size
+                {
+                    // We are starting a new island.
+                    self.active_islands.push(self.active_dynamic_set.len());
+                }
+
+                island_marker = self.stack.len();
+            }
+
+            rb.wake_up();
+            rb.active_island_id = self.active_islands.len() - 1;
+            rb.active_set_id = self.active_dynamic_set.len();
+            rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
+            rb.active_set_timestamp = self.active_set_timestamp;
+            self.active_dynamic_set.push(handle);
+
+            // Read all the contacts and push objects touching this one.
+            for collider_handle in &rb.colliders {
+                let collider = &colliders[*collider_handle];
+
+                for inter in contact_graph.interactions_with(collider.contact_graph_index) {
+                    for manifold in &inter.2.manifolds {
+                        if manifold.num_active_contacts() > 0 {
+                            let other =
+                                crate::utils::other_handle((inter.0, inter.1), *collider_handle);
+                            let other_body = colliders[other].parent;
+                            self.stack.push(other_body);
+                            break;
+                        }
+                    }
+                }
+            }
+
+            for inter in joint_graph.interactions_with(rb.joint_graph_index) {
+                let other = crate::utils::other_handle((inter.0, inter.1), handle);
+                self.stack.push(other);
+            }
+        }
+
+        self.active_islands.push(self.active_dynamic_set.len());
+        //        println!(
+        //            "Extraction: {}, num islands: {}",
+        //            instant::now() - t,
+        //            self.active_islands.len() - 1
+        //        );
+
+        // Actually put to sleep bodies which have not been detected as awake.
+        //        let t = instant::now();
+        for h in &self.can_sleep {
+            let b = &mut self.bodies[*h];
+            if b.activation.sleeping {
+                b.sleep();
+            }
+        }
+        //        println!("Activation: {}", instant::now() - t);
+    }
+}
+
+impl Index<RigidBodyHandle> for RigidBodySet {
+    type Output = RigidBody;
+
+    fn index(&self, index: RigidBodyHandle) -> &RigidBody {
+        &self.bodies[index]
+    }
+}
+
+impl IndexMut<RigidBodyHandle> for RigidBodySet {
+    fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody {
+        &mut self.bodies[index]
+    }
+}

src/dynamics/solver/categorization.rs

@@ -0,0 +1,70 @@
+use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory};
+
+pub(crate) fn categorize_position_contacts(
+    bodies: &RigidBodySet,
+    manifolds: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    out_point_point_ground: &mut Vec<ContactManifoldIndex>,
+    out_plane_point_ground: &mut Vec<ContactManifoldIndex>,
+    out_point_point: &mut Vec<ContactManifoldIndex>,
+    out_plane_point: &mut Vec<ContactManifoldIndex>,
+) {
+    for manifold_i in manifold_indices {
+        let manifold = &manifolds[*manifold_i];
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+
+        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+            match manifold.kinematics.category {
+                KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i),
+                KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i),
+            }
+        } else {
+            match manifold.kinematics.category {
+                KinematicsCategory::PointPoint => out_point_point.push(*manifold_i),
+                KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i),
+            }
+        }
+    }
+}
+
+pub(crate) fn categorize_velocity_contacts(
+    bodies: &RigidBodySet,
+    manifolds: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    out_ground: &mut Vec<ContactManifoldIndex>,
+    out_not_ground: &mut Vec<ContactManifoldIndex>,
+) {
+    for manifold_i in manifold_indices {
+        let manifold = &manifolds[*manifold_i];
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+
+        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+            out_ground.push(*manifold_i)
+        } else {
+            out_not_ground.push(*manifold_i)
+        }
+    }
+}
+
+pub(crate) fn categorize_joints(
+    bodies: &RigidBodySet,
+    joints: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    ground_joints: &mut Vec<JointIndex>,
+    nonground_joints: &mut Vec<JointIndex>,
+) {
+    for joint_i in joint_indices {
+        let joint = &joints[*joint_i].weight;
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
+
+        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+            ground_joints.push(*joint_i);
+        } else {
+            nonground_joints.push(*joint_i);
+        }
+    }
+}

src/dynamics/solver/delta_vel.rs

@@ -0,0 +1,18 @@
+use crate::math::{AngVector, Vector};
+use na::{Scalar, SimdRealField};
+
+#[derive(Copy, Clone, Debug)]
+//#[repr(align(64))]
+pub(crate) struct DeltaVel<N: Scalar> {
+    pub linear: Vector<N>,
+    pub angular: AngVector<N>,
+}
+
+impl<N: SimdRealField> DeltaVel<N> {
+    pub fn zero() -> Self {
+        Self {
+            linear: na::zero(),
+            angular: na::zero(),
+        }
+    }
+}

src/dynamics/solver/interaction_groups.rs

@@ -0,0 +1,434 @@
+use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
+    vec_map::VecMap,
+};
+
+pub(crate) trait PairInteraction {
+    fn body_pair(&self) -> BodyPair;
+}
+
+impl<'a> PairInteraction for &'a mut ContactManifold {
+    fn body_pair(&self) -> BodyPair {
+        self.body_pair
+    }
+}
+
+impl<'a> PairInteraction for JointGraphEdge {
+    fn body_pair(&self) -> BodyPair {
+        BodyPair::new(self.weight.body1, self.weight.body2)
+    }
+}
+
+#[cfg(feature = "parallel")]
+pub(crate) struct ParallelInteractionGroups {
+    bodies_color: Vec<u128>,         // Workspace.
+    interaction_indices: Vec<usize>, // Workspace.
+    interaction_colors: Vec<usize>,  // Workspace.
+    sorted_interactions: Vec<usize>,
+    groups: Vec<usize>,
+}
+
+#[cfg(feature = "parallel")]
+impl ParallelInteractionGroups {
+    pub fn new() -> Self {
+        Self {
+            bodies_color: Vec::new(),
+            interaction_indices: Vec::new(),
+            interaction_colors: Vec::new(),
+            sorted_interactions: Vec::new(),
+            groups: Vec::new(),
+        }
+    }
+
+    pub fn group(&self, i: usize) -> &[usize] {
+        let range = self.groups[i]..self.groups[i + 1];
+        &self.sorted_interactions[range]
+    }
+    pub fn num_groups(&self) -> usize {
+        self.groups.len() - 1
+    }
+
+    pub fn group_interactions<Interaction: PairInteraction>(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        interactions: &[Interaction],
+        interaction_indices: &[usize],
+    ) {
+        let num_island_bodies = bodies.active_island(island_id).len();
+        self.bodies_color.clear();
+        self.interaction_indices.clear();
+        self.groups.clear();
+        self.sorted_interactions.clear();
+        self.interaction_colors.clear();
+
+        let mut color_len = [0; 128];
+        self.bodies_color.resize(num_island_bodies, 0u128);
+        self.interaction_indices
+            .extend_from_slice(interaction_indices);
+        self.interaction_colors.resize(interaction_indices.len(), 0);
+        let bcolors = &mut self.bodies_color;
+
+        for (interaction_id, color) in self
+            .interaction_indices
+            .iter()
+            .zip(self.interaction_colors.iter_mut())
+        {
+            let body_pair = interactions[*interaction_id].body_pair();
+            let rb1 = &bodies[body_pair.body1];
+            let rb2 = &bodies[body_pair.body2];
+
+            match (rb1.is_static(), rb2.is_static()) {
+                (false, false) => {
+                    let color_mask =
+                        bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset];
+                    *color = (!color_mask).trailing_zeros() as usize;
+                    color_len[*color] += 1;
+                    bcolors[rb1.active_set_offset] |= 1 << *color;
+                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                }
+                (true, false) => {
+                    let color_mask = bcolors[rb2.active_set_offset];
+                    *color = (!color_mask).trailing_zeros() as usize;
+                    color_len[*color] += 1;
+                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                }
+                (false, true) => {
+                    let color_mask = bcolors[rb1.active_set_offset];
+                    *color = (!color_mask).trailing_zeros() as usize;
+                    color_len[*color] += 1;
+                    bcolors[rb1.active_set_offset] |= 1 << *color;
+                }
+                (true, true) => unreachable!(),
+            }
+        }
+
+        let mut sort_offsets = [0; 128];
+        let mut last_offset = 0;
+
+        for i in 0..128 {
+            if color_len[i] == 0 {
+                break;
+            }
+
+            self.groups.push(last_offset);
+            sort_offsets[i] = last_offset;
+            last_offset += color_len[i];
+        }
+
+        self.sorted_interactions
+            .resize(interaction_indices.len(), 0);
+
+        for (interaction_id, color) in interaction_indices
+            .iter()
+            .zip(self.interaction_colors.iter())
+        {
+            self.sorted_interactions[sort_offsets[*color]] = *interaction_id;
+            sort_offsets[*color] += 1;
+        }
+
+        self.groups.push(self.sorted_interactions.len());
+    }
+}
+
+pub(crate) struct InteractionGroups {
+    #[cfg(feature = "simd-is-enabled")]
+    buckets: VecMap<([usize; SIMD_WIDTH], usize)>,
+    #[cfg(feature = "simd-is-enabled")]
+    body_masks: Vec<u128>,
+    #[cfg(feature = "simd-is-enabled")]
+    pub grouped_interactions: Vec<usize>,
+    pub nongrouped_interactions: Vec<usize>,
+}
+
+impl InteractionGroups {
+    pub fn new() -> Self {
+        Self {
+            #[cfg(feature = "simd-is-enabled")]
+            buckets: VecMap::new(),
+            #[cfg(feature = "simd-is-enabled")]
+            body_masks: Vec::new(),
+            #[cfg(feature = "simd-is-enabled")]
+            grouped_interactions: Vec::new(),
+            nongrouped_interactions: Vec::new(),
+        }
+    }
+
+    // FIXME: there is a lot of duplicated code with group_manifolds here.
+    // But we don't refactor just now because we may end up with distinct
+    // grouping strategies in the future.
+    #[cfg(not(feature = "simd-is-enabled"))]
+    pub fn group_joints(
+        &mut self,
+        _island_id: usize,
+        _bodies: &RigidBodySet,
+        _interactions: &[JointGraphEdge],
+        interaction_indices: &[JointIndex],
+    ) {
+        self.nongrouped_interactions
+            .extend_from_slice(interaction_indices);
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn group_joints(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        interactions: &[JointGraphEdge],
+        interaction_indices: &[JointIndex],
+    ) {
+        // NOTE: in 3D we have up to 10 different joint types.
+        // In 2D we only have 5 joint types.
+        #[cfg(feature = "dim3")]
+        const NUM_JOINT_TYPES: usize = 10;
+        #[cfg(feature = "dim2")]
+        const NUM_JOINT_TYPES: usize = 5;
+
+        // The j-th bit of joint_type_conflicts[i] indicates that the
+        // j-th bucket contains a joint with a type different than `i`.
+        let mut joint_type_conflicts = [0u128; NUM_JOINT_TYPES];
+
+        // Note: each bit of a body mask indicates what bucket already contains
+        // a constraints involving this body.
+        // FIXME: currently, this is a bit overconservative because when a bucket
+        // is full, we don't clear the corresponding body mask bit. This may result
+        // in less grouped constraints.
+        self.body_masks
+            .resize(bodies.active_island(island_id).len(), 0u128);
+
+        // NOTE: each bit of the occupied mask indicates what bucket already
+        // contains at least one constraint.
+        let mut occupied_mask = 0u128;
+
+        for interaction_i in interaction_indices {
+            let interaction = &interactions[*interaction_i].weight;
+            let body1 = &bodies[interaction.body1];
+            let body2 = &bodies[interaction.body2];
+            let is_static1 = !body1.is_dynamic();
+            let is_static2 = !body2.is_dynamic();
+
+            if is_static1 && is_static2 {
+                continue;
+            }
+
+            let ijoint = interaction.params.type_id();
+            let i1 = body1.active_set_offset;
+            let i2 = body2.active_set_offset;
+            let conflicts =
+                self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
+            let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
+            let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
+
+            let target_index = if conflictfree_occupied_targets != 0 {
+                // Try to fill partial WContacts first.
+                conflictfree_occupied_targets.trailing_zeros()
+            } else {
+                conflictfree_targets.trailing_zeros()
+            };
+
+            if target_index == 128 {
+                // The interaction conflicts with every bucket we can manage.
+                // So push it in an nongrouped interaction list that won't be combined with
+                // any other interactions.
+                self.nongrouped_interactions.push(*interaction_i);
+                continue;
+            }
+
+            let target_mask_bit = 1 << target_index;
+
+            let bucket = self
+                .buckets
+                .entry(target_index as usize)
+                .or_insert_with(|| ([0; SIMD_WIDTH], 0));
+
+            if bucket.1 == SIMD_LAST_INDEX {
+                // We completed our group.
+                (bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
+                self.grouped_interactions.extend_from_slice(&bucket.0);
+                bucket.1 = 0;
+                occupied_mask &= !target_mask_bit;
+
+                for k in 0..NUM_JOINT_TYPES {
+                    joint_type_conflicts[k] &= !target_mask_bit;
+                }
+            } else {
+                (bucket.0)[bucket.1] = *interaction_i;
+                bucket.1 += 1;
+                occupied_mask |= target_mask_bit;
+
+                for k in 0..ijoint {
+                    joint_type_conflicts[k] |= target_mask_bit;
+                }
+                for k in ijoint + 1..NUM_JOINT_TYPES {
+                    joint_type_conflicts[k] |= target_mask_bit;
+                }
+            }
+
+            // NOTE: static bodies don't transmit forces. Therefore they don't
+            // imply any interaction conflicts.
+            if !is_static1 {
+                self.body_masks[i1] |= target_mask_bit;
+            }
+
+            if !is_static2 {
+                self.body_masks[i2] |= target_mask_bit;
+            }
+        }
+
+        self.nongrouped_interactions.extend(
+            self.buckets
+                .values()
+                .flat_map(|e| e.0.iter().take(e.1).copied()),
+        );
+        self.buckets.clear();
+        self.body_masks.iter_mut().for_each(|e| *e = 0);
+
+        assert!(
+            self.grouped_interactions.len() % SIMD_WIDTH == 0,
+            "Invalid SIMD contact grouping."
+        );
+
+        //        println!(
+        //            "Num grouped interactions: {}, nongrouped: {}",
+        //            self.grouped_interactions.len(),
+        //            self.nongrouped_interactions.len()
+        //        );
+    }
+
+    pub fn clear_groups(&mut self) {
+        #[cfg(feature = "simd-is-enabled")]
+        self.grouped_interactions.clear();
+        self.nongrouped_interactions.clear();
+    }
+
+    #[cfg(not(feature = "simd-is-enabled"))]
+    pub fn group_manifolds(
+        &mut self,
+        _island_id: usize,
+        _bodies: &RigidBodySet,
+        _interactions: &[&mut ContactManifold],
+        interaction_indices: &[ContactManifoldIndex],
+    ) {
+        self.nongrouped_interactions
+            .extend_from_slice(interaction_indices);
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn group_manifolds(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        interactions: &[&mut ContactManifold],
+        interaction_indices: &[ContactManifoldIndex],
+    ) {
+        // Note: each bit of a body mask indicates what bucket already contains
+        // a constraints involving this body.
+        // FIXME: currently, this is a bit overconservative because when a bucket
+        // is full, we don't clear the corresponding body mask bit. This may result
+        // in less grouped contacts.
+        // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
+        self.body_masks
+            .resize(bodies.active_island(island_id).len(), 0u128);
+
+        // NOTE: each bit of the occupied mask indicates what bucket already
+        // contains at least one constraint.
+        let mut occupied_mask = 0u128;
+        let max_interaction_points = interaction_indices
+            .iter()
+            .map(|i| interactions[*i].num_active_contacts())
+            .max()
+            .unwrap_or(1);
+
+        // FIXME: find a way to reduce the number of iteration.
+        // There must be a way to iterate just once on every interaction indices
+        // instead of MAX_MANIFOLD_POINTS times.
+        for k in 1..=max_interaction_points {
+            for interaction_i in interaction_indices {
+                let interaction = &interactions[*interaction_i];
+
+                // FIXME: how could we avoid iterating
+                // on each interaction at every iteration on k?
+                if interaction.num_active_contacts() != k {
+                    continue;
+                }
+
+                let body1 = &bodies[interaction.body_pair.body1];
+                let body2 = &bodies[interaction.body_pair.body2];
+                let is_static1 = !body1.is_dynamic();
+                let is_static2 = !body2.is_dynamic();
+
+                // FIXME: don't generate interactions between static bodies in the first place.
+                if is_static1 && is_static2 {
+                    continue;
+                }
+
+                let i1 = body1.active_set_offset;
+                let i2 = body2.active_set_offset;
+                let conflicts = self.body_masks[i1] | self.body_masks[i2];
+                let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
+                let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
+
+                let target_index = if conflictfree_occupied_targets != 0 {
+                    // Try to fill partial WContacts first.
+                    conflictfree_occupied_targets.trailing_zeros()
+                } else {
+                    conflictfree_targets.trailing_zeros()
+                };
+
+                if target_index == 128 {
+                    // The interaction conflicts with every bucket we can manage.
+                    // So push it in an nongrouped interaction list that won't be combined with
+                    // any other interactions.
+                    self.nongrouped_interactions.push(*interaction_i);
+                    continue;
+                }
+
+                let target_mask_bit = 1 << target_index;
+
+                let bucket = self
+                    .buckets
+                    .entry(target_index as usize)
+                    .or_insert_with(|| ([0; SIMD_WIDTH], 0));
+
+                if bucket.1 == SIMD_LAST_INDEX {
+                    // We completed our group.
+                    (bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
+                    self.grouped_interactions.extend_from_slice(&bucket.0);
+                    bucket.1 = 0;
+                    occupied_mask = occupied_mask & (!target_mask_bit);
+                } else {
+                    (bucket.0)[bucket.1] = *interaction_i;
+                    bucket.1 += 1;
+                    occupied_mask = occupied_mask | target_mask_bit;
+                }
+
+                // NOTE: static bodies don't transmit forces. Therefore they don't
+                // imply any interaction conflicts.
+                if !is_static1 {
+                    self.body_masks[i1] |= target_mask_bit;
+                }
+
+                if !is_static2 {
+                    self.body_masks[i2] |= target_mask_bit;
+                }
+            }
+
+            self.nongrouped_interactions.extend(
+                self.buckets
+                    .values()
+                    .flat_map(|e| e.0.iter().take(e.1).copied()),
+            );
+            self.buckets.clear();
+            self.body_masks.iter_mut().for_each(|e| *e = 0);
+            occupied_mask = 0u128;
+        }
+
+        assert!(
+            self.grouped_interactions.len() % SIMD_WIDTH == 0,
+            "Invalid SIMD contact grouping."
+        );
+    }
+}

src/dynamics/solver/island_solver.rs

@@ -0,0 +1,73 @@
+use super::{PositionSolver, VelocitySolver};
+use crate::counters::Counters;
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+
+pub struct IslandSolver {
+    velocity_solver: VelocitySolver,
+    position_solver: PositionSolver,
+}
+
+impl IslandSolver {
+    pub fn new() -> Self {
+        Self {
+            velocity_solver: VelocitySolver::new(),
+            position_solver: PositionSolver::new(),
+        }
+    }
+
+    pub fn solve_island(
+        &mut self,
+        island_id: usize,
+        counters: &mut Counters,
+        params: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+        manifolds: &mut [&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+        joints: &mut [JointGraphEdge],
+        joint_indices: &[JointIndex],
+    ) {
+        if manifold_indices.len() != 0 || joint_indices.len() != 0 {
+            counters.solver.velocity_assembly_time.resume();
+            self.velocity_solver.init_constraints(
+                island_id,
+                params,
+                bodies,
+                manifolds,
+                &manifold_indices,
+                joints,
+                &joint_indices,
+            );
+            counters.solver.velocity_assembly_time.pause();
+
+            counters.solver.velocity_resolution_time.resume();
+            self.velocity_solver
+                .solve_constraints(island_id, params, bodies, manifolds, joints);
+            counters.solver.velocity_resolution_time.pause();
+
+            counters.solver.position_assembly_time.resume();
+            self.position_solver.init_constraints(
+                island_id,
+                params,
+                bodies,
+                manifolds,
+                &manifold_indices,
+                joints,
+                &joint_indices,
+            );
+            counters.solver.position_assembly_time.pause();
+        }
+
+        counters.solver.velocity_update_time.resume();
+        bodies
+            .foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt()));
+        counters.solver.velocity_update_time.pause();
+
+        if manifold_indices.len() != 0 || joint_indices.len() != 0 {
+            counters.solver.position_resolution_time.resume();
+            self.position_solver
+                .solve_constraints(island_id, params, bodies);
+            counters.solver.position_resolution_time.pause();
+        }
+    }
+}

src/dynamics/solver/joint_constraint/ball_position_constraint.rs

@@ -0,0 +1,165 @@
+use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
+#[cfg(feature = "dim2")]
+use crate::math::SdpMatrix;
+use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+
+#[derive(Debug)]
+pub(crate) struct BallPositionConstraint {
+    position1: usize,
+    position2: usize,
+
+    local_com1: Point<f32>,
+    local_com2: Point<f32>,
+
+    im1: f32,
+    im2: f32,
+
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    local_anchor1: Point<f32>,
+    local_anchor2: Point<f32>,
+}
+
+impl BallPositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self {
+        Self {
+            local_com1: rb1.mass_properties.local_com,
+            local_com2: rb2.mass_properties.local_com,
+            im1: rb1.mass_properties.inv_mass,
+            im2: rb2.mass_properties.inv_mass,
+            ii1: rb1.world_inv_inertia_sqrt.squared(),
+            ii2: rb2.world_inv_inertia_sqrt.squared(),
+            local_anchor1: cparams.local_anchor1,
+            local_anchor2: cparams.local_anchor2,
+            position1: rb1.active_set_offset,
+            position2: rb2.active_set_offset,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        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;
+
+        positions[self.position1 as usize] = position1;
+        positions[self.position2 as usize] = position2;
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct BallPositionGroundConstraint {
+    position2: usize,
+    anchor1: Point<f32>,
+    im2: f32,
+    ii2: AngularInertia<f32>,
+    local_anchor2: Point<f32>,
+    local_com2: Point<f32>,
+}
+
+impl BallPositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &BallJoint,
+        flipped: bool,
+    ) -> Self {
+        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: rb1.predicted_position * cparams.local_anchor2,
+                im2: rb2.mass_properties.inv_mass,
+                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                local_anchor2: cparams.local_anchor1,
+                position2: rb2.active_set_offset,
+                local_com2: rb2.mass_properties.local_com,
+            }
+        } else {
+            Self {
+                anchor1: rb1.predicted_position * cparams.local_anchor1,
+                im2: rb2.mass_properties.inv_mass,
+                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                local_anchor2: cparams.local_anchor2,
+                position2: rb2.active_set_offset,
+                local_com2: rb2.mass_properties.local_com,
+            }
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        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;
+        positions[self.position2 as usize] = position2;
+    }
+}

src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs

@@ -0,0 +1,199 @@
+use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
+#[cfg(feature = "dim2")]
+use crate::math::SdpMatrix;
+use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, 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<SimdFloat>,
+    local_com2: Point<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+
+    ii1: AngularInertia<SimdFloat>,
+    ii2: AngularInertia<SimdFloat>,
+
+    local_anchor1: Point<SimdFloat>,
+    local_anchor2: Point<SimdFloat>,
+}
+
+impl WBallPositionConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&BallJoint; SIMD_WIDTH],
+    ) -> Self {
+        let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
+        let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1 = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        )
+        .squared();
+        let ii2 = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        )
+        .squared();
+        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        Self {
+            local_com1,
+            local_com2,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            local_anchor1,
+            local_anchor2,
+            position1,
+            position2,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]);
+        let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
+
+        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 * SimdFloat::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<SimdFloat>,
+    im2: SimdFloat,
+    ii2: AngularInertia<SimdFloat>,
+    local_anchor2: Point<SimdFloat>,
+    local_com2: Point<SimdFloat>,
+}
+
+impl WBallPositionGroundConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&BallJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
+        let anchor1 = position1
+            * Point::from(array![|ii| if flipped[ii] {
+                cparams[ii].local_anchor2
+            } else {
+                cparams[ii].local_anchor1
+            }; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2 = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        )
+        .squared();
+        let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
+            cparams[ii].local_anchor1
+        } else {
+            cparams[ii].local_anchor2
+        }; SIMD_WIDTH]);
+        let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
+
+        Self {
+            anchor1,
+            im2,
+            ii2,
+            local_anchor2,
+            position2,
+            local_com2,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
+
+        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 * SimdFloat::splat(params.joint_erp));
+        position2.translation.vector -= impulse * self.im2;
+
+        let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
+        position2.rotation = Rotation::new(angle2) * position2.rotation;
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.position2[ii]] = position2.extract(ii);
+        }
+    }
+}

src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs

@@ -0,0 +1,238 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{SdpMatrix, Vector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+
+#[derive(Debug)]
+pub(crate) struct BallVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    rhs: Vector<f32>,
+    pub(crate) impulse: Vector<f32>,
+
+    gcross1: Vector<f32>,
+    gcross2: Vector<f32>,
+
+    inv_lhs: SdpMatrix<f32>,
+
+    im1: f32,
+    im2: f32,
+}
+
+impl BallVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &BallJoint,
+    ) -> Self {
+        let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com;
+        let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com;
+
+        let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
+        let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+
+        let rhs = -(vel1 - vel2);
+        let lhs;
+
+        let cmat1 = anchor1.gcross_matrix();
+        let cmat2 = anchor2.gcross_matrix();
+
+        #[cfg(feature = "dim3")]
+        {
+            lhs = rb2
+                .world_inv_inertia_sqrt
+                .squared()
+                .quadform(&cmat2)
+                .add_diagonal(im2)
+                + rb1
+                    .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 = rb1.world_inv_inertia_sqrt.squared();
+            let ii2 = rb2.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 gcross1 = rb1.world_inv_inertia_sqrt.transform_lin_vector(anchor1);
+        let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2);
+
+        let inv_lhs = lhs.inverse_unchecked();
+
+        BallVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            im2,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            gcross1,
+            gcross2,
+            rhs,
+            inv_lhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        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.gcross1.gcross(self.impulse);
+        mj_lambda2.linear -= self.im2 * self.impulse;
+        mj_lambda2.angular -= self.gcross2.gcross(self.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<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1);
+        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        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.gcross1.gcross(impulse);
+
+        mj_lambda2.linear -= self.im2 * impulse;
+        mj_lambda2.angular -= self.gcross2.gcross(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::BallJoint(ball) = &mut joint.params {
+            ball.impulse = self.impulse
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct BallVelocityGroundConstraint {
+    mj_lambda2: usize,
+    joint_id: JointIndex,
+    rhs: Vector<f32>,
+    impulse: Vector<f32>,
+    gcross2: Vector<f32>,
+    inv_lhs: SdpMatrix<f32>,
+    im2: f32,
+}
+
+impl BallVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &BallJoint,
+        flipped: bool,
+    ) -> Self {
+        let (anchor1, anchor2) = if flipped {
+            (
+                rb1.position * cparams.local_anchor2 - rb1.world_com,
+                rb2.position * cparams.local_anchor1 - rb2.world_com,
+            )
+        } else {
+            (
+                rb1.position * cparams.local_anchor1 - rb1.world_com,
+                rb2.position * cparams.local_anchor2 - rb2.world_com,
+            )
+        };
+
+        let im2 = rb2.mass_properties.inv_mass;
+        let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
+        let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
+        let rhs = vel2 - vel1;
+
+        let cmat2 = anchor2.gcross_matrix();
+        let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2);
+
+        let lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            lhs = rb2
+                .world_inv_inertia_sqrt
+                .squared()
+                .quadform(&cmat2)
+                .add_diagonal(im2);
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            let ii2 = rb2.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();
+
+        BallVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            gcross2,
+            rhs,
+            inv_lhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+        mj_lambda2.linear -= self.im2 * self.impulse;
+        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let dvel = vel2 + self.rhs;
+
+        let impulse = self.inv_lhs * dvel;
+        self.impulse += impulse;
+
+        mj_lambda2.linear -= self.im2 * impulse;
+        mj_lambda2.angular -= self.gcross2.gcross(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::BallJoint(ball) = &mut joint.params {
+            ball.impulse = self.impulse
+        }
+    }
+}

src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs

@@ -0,0 +1,329 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdFloat, 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<SimdFloat>,
+    pub(crate) impulse: Vector<SimdFloat>,
+
+    gcross1: Vector<SimdFloat>,
+    gcross2: Vector<SimdFloat>,
+
+    inv_lhs: SdpMatrix<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+}
+
+impl WBallVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&BallJoint; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1 - world_com1;
+        let anchor2 = position2 * local_anchor2 - world_com2;
+
+        let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1);
+        let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2);
+        let rhs = -(vel1 - vel2);
+        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 gcross1 = ii1_sqrt.transform_lin_vector(anchor1);
+        let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
+
+        let inv_lhs = lhs.inverse_unchecked();
+
+        WBallVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            im2,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            gcross1,
+            gcross2,
+            rhs,
+            inv_lhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        mj_lambda1.linear += self.impulse * self.im1;
+        mj_lambda1.angular += self.gcross1.gcross(self.impulse);
+        mj_lambda2.linear -= self.impulse * self.im2;
+        mj_lambda2.angular -= self.gcross2.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<f32>]) {
+        let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1);
+        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        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.gcross1.gcross(impulse);
+
+        mj_lambda2.linear -= impulse * self.im2;
+        mj_lambda2.angular -= self.gcross2.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<SimdFloat>,
+    pub(crate) impulse: Vector<SimdFloat>,
+    gcross2: Vector<SimdFloat>,
+    inv_lhs: SdpMatrix<SimdFloat>,
+    im2: SimdFloat,
+}
+
+impl WBallVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&BallJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let local_anchor1 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+        );
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor2 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+        );
+        let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1 - world_com1;
+        let anchor2 = position2 * local_anchor2 - world_com2;
+
+        let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1);
+        let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2);
+        let rhs = vel2 - vel1;
+        let lhs;
+
+        let cmat2 = anchor2.gcross_matrix();
+        let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
+
+        #[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 * SimdFloat::splat(params.warmstart_coeff),
+            gcross2,
+            rhs,
+            inv_lhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        mj_lambda2.linear -= self.impulse * self.im2;
+        mj_lambda2.angular -= self.gcross2.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<f32>]) {
+        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let dvel = vel2 + self.rhs;
+
+        let impulse = self.inv_lhs * dvel;
+        self.impulse += impulse;
+
+        mj_lambda2.linear -= impulse * self.im2;
+        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::BallJoint(ball) = &mut joint.params {
+                ball.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/fixed_position_constraint.rs

@@ -0,0 +1,139 @@
+use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
+use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::utils::WAngularInertia;
+
+#[derive(Debug)]
+pub(crate) struct FixedPositionConstraint {
+    position1: usize,
+    position2: usize,
+    local_anchor1: Isometry<f32>,
+    local_anchor2: Isometry<f32>,
+    local_com1: Point<f32>,
+    local_com2: Point<f32>,
+    im1: f32,
+    im2: f32,
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    lin_inv_lhs: f32,
+    ang_inv_lhs: AngularInertia<f32>,
+}
+
+impl FixedPositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+        let lin_inv_lhs = 1.0 / (im1 + im2);
+        let ang_inv_lhs = (ii1 + ii2).inverse();
+
+        Self {
+            local_anchor1: cparams.local_anchor1,
+            local_anchor2: cparams.local_anchor2,
+            position1: rb1.active_set_offset,
+            position2: rb2.active_set_offset,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            local_com1: rb1.mass_properties.local_com,
+            local_com2: rb2.mass_properties.local_com,
+            lin_inv_lhs,
+            ang_inv_lhs,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position1 = positions[self.position1 as usize];
+        let mut position2 = positions[self.position2 as usize];
+
+        // Angular correction.
+        let anchor1 = position1 * self.local_anchor1;
+        let anchor2 = position2 * self.local_anchor2;
+        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_anchor1.translation.vector);
+        let anchor2 = position2 * Point::from(self.local_anchor2.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<f32>,
+    local_anchor2: Isometry<f32>,
+    local_com2: Point<f32>,
+    im2: f32,
+    ii2: AngularInertia<f32>,
+    impulse: f32,
+}
+
+impl FixedPositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &FixedJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor1;
+        let local_anchor2;
+
+        if flipped {
+            anchor1 = rb1.predicted_position * cparams.local_anchor2;
+            local_anchor2 = cparams.local_anchor1;
+        } else {
+            anchor1 = rb1.predicted_position * cparams.local_anchor1;
+            local_anchor2 = cparams.local_anchor2;
+        };
+
+        Self {
+            anchor1,
+            local_anchor2,
+            position2: rb2.active_set_offset,
+            im2: rb2.mass_properties.inv_mass,
+            ii2: rb2.world_inv_inertia_sqrt.squared(),
+            local_com2: rb2.mass_properties.local_com,
+            impulse: 0.0,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position2 = positions[self.position2 as usize];
+
+        // Angular correction.
+        let anchor2 = position2 * self.local_anchor2;
+        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_anchor2.translation.vector);
+        let err = anchor2 - anchor1;
+        // NOTE: no need to divide by im2 just to multiply right after.
+        let impulse = err * params.joint_erp;
+        position2.translation.vector -= impulse;
+
+        positions[self.position2 as usize] = position2;
+    }
+}

src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs

@@ -0,0 +1,370 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{AngularInertia, Dim, SpacialVector, Vector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim2")]
+use na::{Matrix3, Vector3};
+#[cfg(feature = "dim3")]
+use na::{Matrix6, Vector6, U3};
+
+#[derive(Debug)]
+pub(crate) struct FixedVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    impulse: SpacialVector<f32>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<f32>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<f32>,
+
+    im1: f32,
+    im2: f32,
+
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    ii1_sqrt: AngularInertia<f32>,
+    ii2_sqrt: AngularInertia<f32>,
+
+    r1: Vector<f32>,
+    r2: Vector<f32>,
+}
+
+impl FixedVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &FixedJoint,
+    ) -> Self {
+        let anchor1 = rb1.position * cparams.local_anchor1;
+        let anchor2 = rb2.position * cparams.local_anchor2;
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1.translation.vector - rb1.world_com.coords;
+        let r2 = anchor2.translation.vector - rb2.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::<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 = 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 = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2);
+        let ang_dvel = -rb1.angvel + rb2.angvel;
+
+        #[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,
+        );
+
+        FixedVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            ii1_sqrt: rb1.world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            inv_lhs,
+            r1,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        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::<U3>(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<f32>]) {
+        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::<U3>(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<f32>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<f32>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<f32>,
+
+    im2: f32,
+    ii2: AngularInertia<f32>,
+    ii2_sqrt: AngularInertia<f32>,
+    r2: Vector<f32>,
+}
+
+impl FixedVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &FixedJoint,
+        flipped: bool,
+    ) -> Self {
+        let (anchor1, anchor2) = if flipped {
+            (
+                rb1.position * cparams.local_anchor2,
+                rb2.position * cparams.local_anchor1,
+            )
+        } else {
+            (
+                rb1.position * cparams.local_anchor1,
+                rb2.position * cparams.local_anchor2,
+            )
+        };
+
+        let r1 = anchor1.translation.vector - rb1.world_com.coords;
+
+        let im2 = rb2.mass_properties.inv_mass;
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r2 = anchor2.translation.vector - rb2.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::<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 = 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 = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
+        let ang_dvel = rb2.angvel - rb1.angvel;
+
+        #[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,
+        );
+
+        FixedVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            ii2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            inv_lhs,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        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::<U3>(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<f32>]) {
+        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::<U3>(3).into_owned();
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        if let JointParams::FixedJoint(fixed) = &mut joint.params {
+            fixed.impulse = self.impulse;
+        }
+    }
+}

src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs

@@ -0,0 +1,472 @@
+use simba::simd::SimdValue;
+
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{
+    AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, SpacialVector, Vector,
+    SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Cholesky, Matrix6, Vector6, U3};
+#[cfg(feature = "dim2")]
+use {
+    crate::utils::SdpMatrix3,
+    na::{Matrix3, Vector3},
+};
+
+#[derive(Debug)]
+pub(crate) struct WFixedVelocityConstraint {
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    impulse: SpacialVector<SimdFloat>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<SimdFloat>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+
+    ii1: AngularInertia<SimdFloat>,
+    ii2: AngularInertia<SimdFloat>,
+
+    ii1_sqrt: AngularInertia<SimdFloat>,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+
+    r1: Vector<SimdFloat>,
+    r2: Vector<SimdFloat>,
+}
+
+impl WFixedVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&FixedJoint; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        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,
+        );
+
+        WFixedVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            ii1_sqrt,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            inv_lhs,
+            r1,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        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<f32>]) {
+        let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        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::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<SimdFloat>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<SimdFloat>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<SimdFloat>,
+
+    im2: SimdFloat,
+    ii2: AngularInertia<SimdFloat>,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+    r2: Vector<SimdFloat>,
+}
+
+impl WFixedVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&FixedJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Isometry::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+        );
+        let local_anchor2 = Isometry::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+        );
+        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        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,
+        );
+
+        WFixedVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2,
+            im2,
+            ii2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            inv_lhs,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        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<f32>]) {
+        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        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::FixedJoint(fixed) = &mut joint.params {
+                fixed.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/joint_constraint.rs

@@ -0,0 +1,340 @@
+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::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
+};
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::SIMD_WIDTH;
+
+pub(crate) enum AnyJointVelocityConstraint {
+    BallConstraint(BallVelocityConstraint),
+    BallGroundConstraint(BallVelocityGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WBallConstraint(WBallVelocityConstraint),
+    #[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),
+    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.
+    Empty,
+}
+
+impl AnyJointVelocityConstraint {
+    #[cfg(feature = "parallel")]
+    pub fn num_active_constraints(_: &Joint) -> usize {
+        1
+    }
+
+    pub fn from_joint(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        joint: &Joint,
+        bodies: &RigidBodySet,
+    ) -> Self {
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
+
+        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),
+            ),
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
+                RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
+            ),
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn from_wide_joint(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        joints: [&Joint; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+    ) -> Self {
+        let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
+        let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+
+        match &joints[0].params {
+            JointParams::BallJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params(
+                    params, joint_id, rbs1, rbs2, joints,
+                ))
+            }
+            JointParams::FixedJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params(
+                    params, joint_id, rbs1, rbs2, joints,
+                ))
+            }
+            JointParams::PrismaticJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WPrismaticConstraint(
+                    WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
+                )
+            }
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WRevoluteConstraint(
+                    WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
+                )
+            }
+        }
+    }
+
+    pub fn from_joint_ground(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        joint: &Joint,
+        bodies: &RigidBodySet,
+    ) -> Self {
+        let mut rb1 = &bodies[joint.body1];
+        let mut rb2 = &bodies[joint.body2];
+        let flipped = !rb2.is_dynamic();
+
+        if flipped {
+            std::mem::swap(&mut rb1, &mut rb2);
+        }
+
+        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::PrismaticJoint(p) => {
+                AnyJointVelocityConstraint::PrismaticGroundConstraint(
+                    PrismaticVelocityGroundConstraint::from_params(
+                        params, joint_id, rb1, rb2, p, flipped,
+                    ),
+                )
+            }
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint(
+                RevoluteVelocityGroundConstraint::from_params(
+                    params, joint_id, rb1, rb2, p, flipped,
+                ),
+            ),
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn from_wide_joint_ground(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        joints: [&Joint; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+    ) -> Self {
+        let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
+        let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+        let mut flipped = [false; SIMD_WIDTH];
+
+        for ii in 0..SIMD_WIDTH {
+            if !rbs2[ii].is_dynamic() {
+                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+                flipped[ii] = true;
+            }
+        }
+
+        match &joints[0].params {
+            JointParams::BallJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WBallGroundConstraint(
+                    WBallVelocityGroundConstraint::from_params(
+                        params, joint_id, rbs1, rbs2, joints, flipped,
+                    ),
+                )
+            }
+            JointParams::FixedJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WFixedGroundConstraint(
+                    WFixedVelocityGroundConstraint::from_params(
+                        params, joint_id, rbs1, rbs2, joints, flipped,
+                    ),
+                )
+            }
+            JointParams::PrismaticJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WPrismaticGroundConstraint(
+                    WPrismaticVelocityGroundConstraint::from_params(
+                        params, joint_id, rbs1, rbs2, joints, flipped,
+                    ),
+                )
+            }
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WRevoluteGroundConstraint(
+                    WRevoluteVelocityGroundConstraint::from_params(
+                        params, joint_id, rbs1, rbs2, joints, flipped,
+                    ),
+                )
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        match self {
+            AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WBallConstraint(c) => c.warmstart(mj_lambdas),
+            #[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::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::Empty => unreachable!(),
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        match self {
+            AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WBallConstraint(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::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::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) => {
+                c.writeback_impulses(joints_all)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WFixedConstraint(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
+            AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
+            AnyJointVelocityConstraint::PrismaticGroundConstraint(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) => {
+                c.writeback_impulses(joints_all)
+            }
+            AnyJointVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+}

src/dynamics/solver/joint_constraint/joint_position_constraint.rs

@@ -0,0 +1,169 @@
+use super::{
+    BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
+    FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
+};
+#[cfg(feature = "dim3")]
+use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
+#[cfg(feature = "simd-is-enabled")]
+use super::{WBallPositionConstraint, WBallPositionGroundConstraint};
+use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
+use crate::math::Isometry;
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::SIMD_WIDTH;
+
+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),
+    PrismaticJoint(PrismaticPositionConstraint),
+    PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
+    #[cfg(feature = "dim3")]
+    RevoluteJoint(RevolutePositionConstraint),
+    #[cfg(feature = "dim3")]
+    RevoluteGroundConstraint(RevolutePositionGroundConstraint),
+    #[allow(dead_code)] // The Empty variant is only used with parallel code.
+    Empty,
+}
+
+impl AnyJointPositionConstraint {
+    #[cfg(feature = "parallel")]
+    pub fn num_active_constraints(joint: &Joint, grouped: bool) -> usize {
+        #[cfg(feature = "simd-is-enabled")]
+        if !grouped {
+            1
+        } else {
+            match &joint.params {
+                JointParams::BallJoint(_) => 1,
+                _ => SIMD_WIDTH, // For joints that don't support SIMD position constraints yet.
+            }
+        }
+
+        #[cfg(not(feature = "simd-is-enabled"))]
+        {
+            1
+        }
+    }
+
+    pub fn from_joint(joint: &Joint, bodies: &RigidBodySet) -> Self {
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
+
+        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::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(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Option<Self> {
+        let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
+        let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+
+        match &joints[0].params {
+            JointParams::BallJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                Some(AnyJointPositionConstraint::WBallJoint(
+                    WBallPositionConstraint::from_params(rbs1, rbs2, joints),
+                ))
+            }
+            JointParams::FixedJoint(_) => None,
+            JointParams::PrismaticJoint(_) => None,
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => None,
+        }
+    }
+
+    pub fn from_joint_ground(joint: &Joint, bodies: &RigidBodySet) -> Self {
+        let mut rb1 = &bodies[joint.body1];
+        let mut rb2 = &bodies[joint.body2];
+        let flipped = !rb2.is_dynamic();
+
+        if flipped {
+            std::mem::swap(&mut rb1, &mut rb2);
+        }
+
+        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::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(
+        joints: [&Joint; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+    ) -> Option<Self> {
+        let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
+        let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+        let mut flipped = [false; SIMD_WIDTH];
+
+        for ii in 0..SIMD_WIDTH {
+            if !rbs2[ii].is_dynamic() {
+                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+                flipped[ii] = true;
+            }
+        }
+
+        match &joints[0].params {
+            JointParams::BallJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                Some(AnyJointPositionConstraint::WBallGroundConstraint(
+                    WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
+                ))
+            }
+            JointParams::FixedJoint(_) => None,
+            JointParams::PrismaticJoint(_) => None,
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => None,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        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),
+            AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::PrismaticGroundConstraint(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),
+            AnyJointPositionConstraint::Empty => unreachable!(),
+        }
+    }
+}

src/dynamics/solver/joint_constraint/mod.rs

@@ -0,0 +1,65 @@
+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};
+pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint};
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use fixed_velocity_constraint_wide::{
+    WFixedVelocityConstraint, WFixedVelocityGroundConstraint,
+};
+pub(crate) use joint_constraint::AnyJointVelocityConstraint;
+pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
+pub(self) use prismatic_position_constraint::{
+    PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
+};
+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(feature = "dim3")]
+pub(self) use revolute_velocity_constraint::{
+    RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint,
+};
+#[cfg(feature = "dim3")]
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use revolute_velocity_constraint_wide::{
+    WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint,
+};
+
+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;
+mod fixed_velocity_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod fixed_velocity_constraint_wide;
+mod joint_constraint;
+mod joint_position_constraint;
+mod prismatic_position_constraint;
+mod prismatic_velocity_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod prismatic_velocity_constraint_wide;
+#[cfg(feature = "dim3")]
+mod revolute_position_constraint;
+#[cfg(feature = "dim3")]
+mod revolute_velocity_constraint;
+#[cfg(feature = "dim3")]
+#[cfg(feature = "simd-is-enabled")]
+mod revolute_velocity_constraint_wide;

src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs

@@ -0,0 +1,170 @@
+use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
+use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
+use crate::utils::WAngularInertia;
+use na::Unit;
+
+#[derive(Debug)]
+pub(crate) struct PrismaticPositionConstraint {
+    position1: usize,
+    position2: usize,
+
+    im1: f32,
+    im2: f32,
+
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    lin_inv_lhs: f32,
+    ang_inv_lhs: AngularInertia<f32>,
+
+    limits: [f32; 2],
+
+    local_frame1: Isometry<f32>,
+    local_frame2: Isometry<f32>,
+
+    local_axis1: Unit<Vector<f32>>,
+    local_axis2: Unit<Vector<f32>>,
+}
+
+impl PrismaticPositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self {
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.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: rb1.active_set_offset,
+            position2: rb2.active_set_offset,
+            limits: cparams.limits,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        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<f32>,
+    local_frame2: Isometry<f32>,
+    axis1: Unit<Vector<f32>>,
+    local_axis2: Unit<Vector<f32>>,
+    limits: [f32; 2],
+}
+
+impl PrismaticPositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &PrismaticJoint,
+        flipped: bool,
+    ) -> Self {
+        let frame1;
+        let local_frame2;
+        let axis1;
+        let local_axis2;
+
+        if flipped {
+            frame1 = rb1.predicted_position * cparams.local_frame2();
+            local_frame2 = cparams.local_frame1();
+            axis1 = rb1.predicted_position * cparams.local_axis2;
+            local_axis2 = cparams.local_axis1;
+        } else {
+            frame1 = rb1.predicted_position * cparams.local_frame1();
+            local_frame2 = cparams.local_frame2();
+            axis1 = rb1.predicted_position * cparams.local_axis1;
+            local_axis2 = cparams.local_axis2;
+        };
+
+        Self {
+            frame1,
+            local_frame2,
+            axis1,
+            local_axis2,
+            position2: rb2.active_set_offset,
+            limits: cparams.limits,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position2 = positions[self.position2 as usize];
+
+        // Angular correction.
+        let frame2 = position2 * self.local_frame2;
+        let ang_err = frame2.rotation * self.frame1.rotation.inverse();
+        position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
+
+        // Linear correction.
+        let anchor1 = Point::from(self.frame1.translation.vector);
+        let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
+        let dpos = anchor2 - anchor1;
+        let limit_err = dpos.dot(&self.axis1);
+        let mut err = dpos - *self.axis1 * limit_err;
+
+        if limit_err < self.limits[0] {
+            err += *self.axis1 * (limit_err - self.limits[0]);
+        } else if limit_err > self.limits[1] {
+            err += *self.axis1 * (limit_err - self.limits[1]);
+        }
+
+        // NOTE: no need to divide by im2 just to multiply right after.
+        let impulse = err * params.joint_erp;
+        position2.translation.vector -= impulse;
+
+        positions[self.position2 as usize] = position2;
+    }
+}

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

@@ -0,0 +1,558 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
+};
+use crate::math::{AngularInertia, Vector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+#[cfg(feature = "dim2")]
+use {
+    crate::utils::SdpMatrix2,
+    na::{Matrix2, Vector2},
+};
+
+#[cfg(feature = "dim2")]
+type LinImpulseDim = na::U1;
+#[cfg(feature = "dim3")]
+type LinImpulseDim = na::U2;
+
+#[derive(Debug)]
+pub(crate) struct PrismaticVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    r1: Vector<f32>,
+    r2: Vector<f32>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix5<f32>,
+    #[cfg(feature = "dim3")]
+    rhs: Vector5<f32>,
+    #[cfg(feature = "dim3")]
+    impulse: Vector5<f32>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix2<f32>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector2<f32>,
+    #[cfg(feature = "dim2")]
+    impulse: Vector2<f32>,
+
+    limits_impulse: f32,
+    limits_forcedirs: Option<(Vector<f32>, Vector<f32>)>,
+    limits_rhs: f32,
+
+    #[cfg(feature = "dim2")]
+    basis1: Vector2<f32>,
+    #[cfg(feature = "dim3")]
+    basis1: Matrix3x2<f32>,
+
+    im1: f32,
+    im2: f32,
+
+    ii1_sqrt: AngularInertia<f32>,
+    ii2_sqrt: AngularInertia<f32>,
+}
+
+impl PrismaticVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &PrismaticJoint,
+    ) -> Self {
+        // Linear part.
+        let anchor1 = rb1.position * cparams.local_anchor1;
+        let anchor2 = rb2.position * cparams.local_anchor2;
+        let axis1 = rb1.position * cparams.local_axis1;
+        let axis2 = rb2.position * cparams.local_axis2;
+        #[cfg(feature = "dim2")]
+        let basis1 = rb1.position * cparams.basis1[0];
+        #[cfg(feature = "dim3")]
+        let basis1 = Matrix3x2::from_columns(&[
+            rb1.position * cparams.basis1[0],
+            rb1.position * cparams.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(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 im1 = rb1.mass_properties.inv_mass;
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - rb1.world_com;
+        let r1_mat = r1.gcross_matrix();
+
+        let im2 = rb2.mass_properties.inv_mass;
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r2 = anchor2 - rb2.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::<U2, U2>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(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 = rb1.linvel + rb1.angvel.gcross(r1);
+        let anchor_linvel2 = rb2.linvel + rb2.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 lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let ang_rhs = rb2.angvel - rb1.angvel;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+
+        // Setup limit constraint.
+        let mut limits_forcedirs = None;
+        let mut limits_rhs = 0.0;
+        let mut limits_impulse = 0.0;
+
+        if cparams.limits_enabled {
+            let danchor = anchor2 - anchor1;
+            let dist = danchor.dot(&axis1);
+
+            // FIXME: we should allow both limits to be active at
+            // the same time, and allow predictive constraint activation.
+            if dist < cparams.limits[0] {
+                limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner()));
+                limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
+                limits_impulse = cparams.limits_impulse;
+            } else if dist > cparams.limits[1] {
+                limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner()));
+                limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
+                limits_impulse = cparams.limits_impulse;
+            }
+        }
+
+        PrismaticVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            ii1_sqrt: rb1.world_inv_inertia_sqrt,
+            im2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            limits_impulse: limits_impulse * params.warmstart_coeff,
+            limits_forcedirs,
+            limits_rhs,
+            basis1,
+            inv_lhs,
+            rhs,
+            r1,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        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::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(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));
+
+        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+            mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_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<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        /*
+         * Joint consraint.
+         */
+        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::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(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));
+
+        /*
+         * Joint limits.
+         */
+        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+            // FIXME: the transformation by ii2_sqrt could be avoided by
+            // reusing some computations above.
+            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.im1 + self.im2)).max(0.0);
+            let dimpulse = new_impulse - self.limits_impulse;
+            self.limits_impulse = new_impulse;
+
+            mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+        }
+
+        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.limits_impulse = self.limits_impulse;
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct PrismaticVelocityGroundConstraint {
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    r2: Vector<f32>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix2<f32>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector2<f32>,
+    #[cfg(feature = "dim2")]
+    impulse: Vector2<f32>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix5<f32>,
+    #[cfg(feature = "dim3")]
+    rhs: Vector5<f32>,
+    #[cfg(feature = "dim3")]
+    impulse: Vector5<f32>,
+
+    limits_impulse: f32,
+    limits_rhs: f32,
+
+    axis2: Vector<f32>,
+    #[cfg(feature = "dim2")]
+    basis1: Vector2<f32>,
+    #[cfg(feature = "dim3")]
+    basis1: Matrix3x2<f32>,
+    limits_forcedir2: Option<Vector<f32>>,
+
+    im2: f32,
+    ii2_sqrt: AngularInertia<f32>,
+}
+
+impl PrismaticVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &PrismaticJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor2;
+        let anchor1;
+        let axis2;
+        let axis1;
+        let basis1;
+
+        if flipped {
+            anchor2 = rb2.position * cparams.local_anchor1;
+            anchor1 = rb1.position * cparams.local_anchor2;
+            axis2 = rb2.position * cparams.local_axis1;
+            axis1 = rb1.position * cparams.local_axis2;
+            #[cfg(feature = "dim2")]
+            {
+                basis1 = rb1.position * cparams.basis2[0];
+            }
+            #[cfg(feature = "dim3")]
+            {
+                basis1 = Matrix3x2::from_columns(&[
+                    rb1.position * cparams.basis2[0],
+                    rb1.position * cparams.basis2[1],
+                ]);
+            }
+        } else {
+            anchor2 = rb2.position * cparams.local_anchor2;
+            anchor1 = rb1.position * cparams.local_anchor1;
+            axis2 = rb2.position * cparams.local_axis2;
+            axis1 = rb1.position * cparams.local_axis1;
+            #[cfg(feature = "dim2")]
+            {
+                basis1 = rb1.position * cparams.basis1[0];
+            }
+            #[cfg(feature = "dim3")]
+            {
+                basis1 = Matrix3x2::from_columns(&[
+                    rb1.position * cparams.basis1[0],
+                    rb1.position * cparams.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(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 = rb2.mass_properties.inv_mass;
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - rb1.world_com;
+        let r2 = anchor2 - rb2.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::<U2, U2>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(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 = rb1.linvel + rb1.angvel.gcross(r1);
+        let anchor_linvel2 = rb2.linvel + rb2.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 lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let ang_rhs = rb2.angvel - rb1.angvel;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+
+        // Setup limit constraint.
+        let mut limits_forcedir2 = None;
+        let mut limits_rhs = 0.0;
+        let mut limits_impulse = 0.0;
+
+        if cparams.limits_enabled {
+            let danchor = anchor2 - anchor1;
+            let dist = danchor.dot(&axis1);
+
+            // FIXME: we should allow both limits to be active at
+            // the same time.
+            // FIXME: allow predictive constraint activation.
+            if dist < cparams.limits[0] {
+                limits_forcedir2 = Some(axis2.into_inner());
+                limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
+                limits_impulse = cparams.limits_impulse;
+            } else if dist > cparams.limits[1] {
+                limits_forcedir2 = Some(-axis2.into_inner());
+                limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
+                limits_impulse = cparams.limits_impulse;
+            }
+        }
+
+        PrismaticVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            limits_impulse: limits_impulse * params.warmstart_coeff,
+            basis1,
+            inv_lhs,
+            rhs,
+            r2,
+            axis2: axis2.into_inner(),
+            limits_forcedir2,
+            limits_rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(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));
+
+        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
+        }
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        /*
+         * Joint consraint.
+         */
+        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::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(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));
+
+        /*
+         * Joint limits.
+         */
+        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+            // 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 = 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(0.0);
+            let dimpulse = new_impulse - self.limits_impulse;
+            self.limits_impulse = new_impulse;
+
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+        }
+
+        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::PrismaticJoint(revolute) = &mut joint.params {
+            revolute.impulse = self.impulse;
+            revolute.limits_impulse = self.limits_impulse;
+        }
+    }
+}

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs

@@ -0,0 +1,687 @@
+use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
+
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
+};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, SimdBool, SimdFloat, Vector, SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+#[cfg(feature = "dim2")]
+use {
+    crate::utils::SdpMatrix2,
+    na::{Matrix2, Vector2},
+};
+
+#[cfg(feature = "dim2")]
+type LinImpulseDim = na::U1;
+#[cfg(feature = "dim3")]
+type LinImpulseDim = na::U2;
+
+#[derive(Debug)]
+pub(crate) struct WPrismaticVelocityConstraint {
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    r1: Vector<SimdFloat>,
+    r2: Vector<SimdFloat>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix5<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    rhs: Vector5<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    impulse: Vector5<SimdFloat>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix2<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector2<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    impulse: Vector2<SimdFloat>,
+
+    limits_impulse: SimdFloat,
+    limits_forcedirs: Option<(Vector<SimdFloat>, Vector<SimdFloat>)>,
+    limits_rhs: SimdFloat,
+
+    #[cfg(feature = "dim2")]
+    basis1: Vector2<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    basis1: Matrix3x2<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+
+    ii1_sqrt: AngularInertia<SimdFloat>,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+}
+
+impl WPrismaticVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&PrismaticJoint; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]);
+        let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]);
+
+        #[cfg(feature = "dim2")]
+        let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])];
+        #[cfg(feature = "dim3")]
+        let local_basis1 = [
+            Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
+        ];
+
+        #[cfg(feature = "dim2")]
+        let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        #[cfg(feature = "dim3")]
+        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        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(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::<U2, U2>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(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 lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let ang_rhs = angvel2 - angvel1;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+
+        // Setup limit constraint.
+        let mut limits_forcedirs = None;
+        let mut limits_rhs = na::zero();
+        let mut limits_impulse = na::zero();
+        let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
+
+        if limits_enabled.any() {
+            let danchor = anchor2 - anchor1;
+            let dist = danchor.dot(&axis1);
+
+            // FIXME: we should allow both limits to be active at
+            // the same time + allow predictive constraint activation.
+            let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
+            let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
+            let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
+
+            let min_enabled = dist.simd_lt(min_limit);
+            let max_enabled = dist.simd_gt(max_limit);
+            let _0: SimdFloat = na::zero();
+            let _1: SimdFloat = na::one();
+            let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0));
+
+            if sign != _0 {
+                limits_forcedirs = Some((axis1 * -sign, axis2 * sign));
+                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign;
+                limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0);
+            }
+        }
+
+        WPrismaticVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            ii1_sqrt,
+            im2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
+            limits_forcedirs,
+            limits_rhs,
+            basis1,
+            inv_lhs,
+            rhs,
+            r1,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(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));
+
+        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+            mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_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<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        /*
+         * Joint consraint.
+         */
+        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::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(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));
+
+        /*
+         * Joint limits.
+         */
+        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+            // FIXME: the transformation by ii2_sqrt could be avoided by
+            // reusing some computations above.
+            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.im1 + self.im2)).simd_max(na::zero());
+            let dimpulse = new_impulse - self.limits_impulse;
+            self.limits_impulse = new_impulse;
+
+            mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+        }
+
+        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<SimdFloat>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix2<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector2<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    impulse: Vector2<SimdFloat>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix5<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    rhs: Vector5<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    impulse: Vector5<SimdFloat>,
+
+    limits_impulse: SimdFloat,
+    limits_rhs: SimdFloat,
+
+    axis2: Vector<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    basis1: Vector2<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    basis1: Matrix3x2<SimdFloat>,
+    limits_forcedir2: Option<Vector<SimdFloat>>,
+
+    im2: SimdFloat,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+}
+
+impl WPrismaticVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&PrismaticJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        #[cfg(feature = "dim2")]
+        let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        #[cfg(feature = "dim3")]
+        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let local_anchor1 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+        );
+        let local_anchor2 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+        );
+        let local_axis1 = Vector::from(
+            array![|ii| if flipped[ii] { *cparams[ii].local_axis2 } else { *cparams[ii].local_axis1 }; SIMD_WIDTH],
+        );
+        let local_axis2 = Vector::from(
+            array![|ii| if flipped[ii] { *cparams[ii].local_axis1 } else { *cparams[ii].local_axis2 }; SIMD_WIDTH],
+        );
+
+        #[cfg(feature = "dim2")]
+        let basis1 = position1
+            * Vector::from(
+                array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
+            );
+        #[cfg(feature = "dim3")]
+        let basis1 = Matrix3x2::from_columns(&[
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
+                ),
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
+                ),
+        ]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+        let axis1 = position1 * local_axis1;
+        let axis2 = position2 * local_axis2;
+
+        // #[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(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 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::<U2, U2>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(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 lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let ang_rhs = angvel2 - angvel1;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+
+        // Setup limit constraint.
+        let mut limits_forcedir2 = None;
+        let mut limits_rhs = na::zero();
+        let mut limits_impulse = na::zero();
+        let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
+
+        if limits_enabled.any() {
+            let danchor = anchor2 - anchor1;
+            let dist = danchor.dot(&axis1);
+
+            // FIXME: we should allow both limits to be active at
+            // the same time + allow predictive constraint activation.
+            let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
+            let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
+            let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
+
+            let use_min = dist.simd_lt(min_limit);
+            let use_max = dist.simd_gt(max_limit);
+            let _0: SimdFloat = na::zero();
+            let _1: SimdFloat = na::one();
+            let sign = _1.select(use_min, (-_1).select(use_max, _0));
+
+            if sign != _0 {
+                limits_forcedir2 = Some(axis2 * sign);
+                limits_rhs = anchor_linvel2.dot(&axis2) * sign - anchor_linvel1.dot(&axis1) * sign;
+                limits_impulse = lim_impulse.select(use_min | use_max, _0);
+            }
+        }
+
+        WPrismaticVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2,
+            im2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
+            basis1,
+            inv_lhs,
+            rhs,
+            r2,
+            axis2,
+            limits_forcedir2,
+            limits_rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(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));
+
+        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+            mj_lambda2.linear += 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);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        /*
+         * Joint consraint.
+         */
+        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::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(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));
+
+        /*
+         * Joint limits.
+         */
+        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+            // 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 = 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(na::zero());
+            let dimpulse = new_impulse - self.limits_impulse;
+            self.limits_impulse = new_impulse;
+
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+        }
+
+        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::PrismaticJoint(rev) = &mut joint.params {
+                rev.impulse = self.impulse.extract(ii);
+                rev.limits_impulse = self.limits_impulse.extract(ii);
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/revolute_position_constraint.rs

@@ -0,0 +1,142 @@
+use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
+use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
+use crate::utils::WAngularInertia;
+use na::Unit;
+
+#[derive(Debug)]
+pub(crate) struct RevolutePositionConstraint {
+    position1: usize,
+    position2: usize,
+
+    im1: f32,
+    im2: f32,
+
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    lin_inv_lhs: f32,
+    ang_inv_lhs: AngularInertia<f32>,
+
+    local_anchor1: Point<f32>,
+    local_anchor2: Point<f32>,
+
+    local_axis1: Unit<Vector<f32>>,
+    local_axis2: Unit<Vector<f32>>,
+}
+
+impl RevolutePositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self {
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.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_anchor1: cparams.local_anchor1,
+            local_anchor2: cparams.local_anchor2,
+            local_axis1: cparams.local_axis1,
+            local_axis2: cparams.local_axis2,
+            position1: rb1.active_set_offset,
+            position2: rb2.active_set_offset,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position1 = positions[self.position1 as usize];
+        let mut position2 = positions[self.position2 as usize];
+
+        let axis1 = position1 * self.local_axis1;
+        let axis2 = position2 * self.local_axis2;
+        let delta_rot =
+            Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or(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;
+
+        let anchor1 = position1 * self.local_anchor1;
+        let anchor2 = position2 * self.local_anchor2;
+
+        let delta_tra = anchor2 - anchor1;
+        let lin_error = delta_tra * params.joint_erp;
+        let lin_impulse = self.lin_inv_lhs * lin_error;
+
+        position1.translation.vector += self.im1 * lin_impulse;
+        position2.translation.vector -= self.im2 * lin_impulse;
+
+        positions[self.position1 as usize] = position1;
+        positions[self.position2 as usize] = position2;
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct RevolutePositionGroundConstraint {
+    position2: usize,
+    anchor1: Point<f32>,
+    local_anchor2: Point<f32>,
+    axis1: Unit<Vector<f32>>,
+    local_axis2: Unit<Vector<f32>>,
+}
+
+impl RevolutePositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &RevoluteJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor1;
+        let local_anchor2;
+        let axis1;
+        let local_axis2;
+
+        if flipped {
+            anchor1 = rb1.predicted_position * cparams.local_anchor2;
+            local_anchor2 = cparams.local_anchor1;
+            axis1 = rb1.predicted_position * cparams.local_axis2;
+            local_axis2 = cparams.local_axis1;
+        } else {
+            anchor1 = rb1.predicted_position * cparams.local_anchor1;
+            local_anchor2 = cparams.local_anchor2;
+            axis1 = rb1.predicted_position * cparams.local_axis1;
+            local_axis2 = cparams.local_axis2;
+        };
+
+        Self {
+            anchor1,
+            local_anchor2,
+            axis1,
+            local_axis2,
+            position2: rb2.active_set_offset,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position2 = positions[self.position2 as usize];
+
+        let axis2 = position2 * self.local_axis2;
+
+        let delta_rot =
+            Rotation::scaled_rotation_between_axis(&axis2, &self.axis1, params.joint_erp)
+                .unwrap_or(Rotation::identity());
+        position2.rotation = delta_rot * position2.rotation;
+
+        let anchor2 = position2 * self.local_anchor2;
+        let delta_tra = anchor2 - self.anchor1;
+        let lin_error = delta_tra * params.joint_erp;
+        position2.translation.vector -= lin_error;
+
+        positions[self.position2 as usize] = position2;
+    }
+}

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -0,0 +1,294 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
+};
+use crate::math::{AngularInertia, Vector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+
+#[derive(Debug)]
+pub(crate) struct RevoluteVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    r1: Vector<f32>,
+    r2: Vector<f32>,
+
+    inv_lhs: Matrix5<f32>,
+    rhs: Vector5<f32>,
+    impulse: Vector5<f32>,
+
+    basis1: Matrix3x2<f32>,
+
+    im1: f32,
+    im2: f32,
+
+    ii1_sqrt: AngularInertia<f32>,
+    ii2_sqrt: AngularInertia<f32>,
+}
+
+impl RevoluteVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &RevoluteJoint,
+    ) -> Self {
+        // Linear part.
+        let anchor1 = rb1.position * cparams.local_anchor1;
+        let anchor2 = rb2.position * cparams.local_anchor2;
+        let basis1 = Matrix3x2::from_columns(&[
+            rb1.position * cparams.basis1[0],
+            rb1.position * cparams.basis1[1],
+        ]);
+
+        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //            .unwrap_or(Rotation::identity())
+        //            .to_rotation_matrix()
+        //            .into_inner();
+        //        let basis2 = r21 * basis1;
+        // NOTE: to simplify, we use basis2 = basis1.
+        // Though we may want to test if that does not introduce any instability.
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - rb1.world_com;
+        let r1_mat = r1.gcross_matrix();
+
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r2 = anchor2 - rb2.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 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
+        let lhs11 = (ii1 + ii2).quadform3x2(&basis1).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::<U3, U3>(0, 0)
+            .copy_from(&lhs00.into_matrix());
+        lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
+        lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
+
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
+        let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        RevoluteVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            ii1_sqrt: rb1.world_inv_inertia_sqrt,
+            basis1,
+            im2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            inv_lhs,
+            rhs,
+            r1,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        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::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(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<f32>]) {
+        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 lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2)
+            - mj_lambda1.linear
+            - ang_vel1.gcross(self.r1);
+        let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - 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_impulse = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(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::RevoluteJoint(revolute) = &mut joint.params {
+            revolute.impulse = self.impulse;
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct RevoluteVelocityGroundConstraint {
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    r2: Vector<f32>,
+
+    inv_lhs: Matrix5<f32>,
+    rhs: Vector5<f32>,
+    impulse: Vector5<f32>,
+
+    basis1: Matrix3x2<f32>,
+
+    im2: f32,
+
+    ii2_sqrt: AngularInertia<f32>,
+}
+
+impl RevoluteVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &RevoluteJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor2;
+        let anchor1;
+        let basis1;
+
+        if flipped {
+            anchor1 = rb1.position * cparams.local_anchor2;
+            anchor2 = rb2.position * cparams.local_anchor1;
+            basis1 = Matrix3x2::from_columns(&[
+                rb1.position * cparams.basis2[0],
+                rb1.position * cparams.basis2[1],
+            ]);
+        } else {
+            anchor1 = rb1.position * cparams.local_anchor1;
+            anchor2 = rb2.position * cparams.local_anchor2;
+            basis1 = Matrix3x2::from_columns(&[
+                rb1.position * cparams.basis1[0],
+                rb1.position * cparams.basis1[1],
+            ]);
+        };
+
+        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //            .unwrap_or(Rotation::identity())
+        //            .to_rotation_matrix()
+        //            .into_inner();
+        //        let basis2 = /*r21 * */ basis1;
+        let im2 = rb2.mass_properties.inv_mass;
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - rb1.world_com;
+        let r2 = anchor2 - rb2.world_com;
+        let r2_mat = r2.gcross_matrix();
+
+        let mut lhs = Matrix5::zeros();
+        let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
+        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
+        let lhs11 = ii2.quadform3x2(&basis1).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::<U3, U3>(0, 0)
+            .copy_from(&lhs00.into_matrix());
+        lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
+        lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
+
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
+        let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        RevoluteVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            basis1,
+            inv_lhs,
+            rhs,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(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<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        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.basis1.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::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(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::RevoluteJoint(revolute) = &mut joint.params {
+            revolute.impulse = self.impulse;
+        }
+    }
+}

src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs

@@ -0,0 +1,397 @@
+use simba::simd::SimdValue;
+
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
+};
+use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, SIMD_WIDTH};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+
+#[derive(Debug)]
+pub(crate) struct WRevoluteVelocityConstraint {
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    r1: Vector<SimdFloat>,
+    r2: Vector<SimdFloat>,
+
+    inv_lhs: Matrix5<SimdFloat>,
+    rhs: Vector5<SimdFloat>,
+    impulse: Vector5<SimdFloat>,
+
+    basis1: Matrix3x2<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+
+    ii1_sqrt: AngularInertia<SimdFloat>,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+}
+
+impl WRevoluteVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&RevoluteJoint; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let local_basis1 = [
+            Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
+        ];
+        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+        let basis1 =
+            Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
+
+        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //            .unwrap_or(Rotation::identity())
+        //            .to_rotation_matrix()
+        //            .into_inner();
+        //        let basis2 = r21 * basis1;
+        // NOTE: to simplify, we use basis2 = basis1.
+        // Though we may want to test if that does not introduce any instability.
+        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 + ii1 * r1_mat));
+        let lhs11 = (ii1 + ii2).quadform3x2(&basis1).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::<U3, U3>(0, 0)
+            .copy_from(&lhs00.into_matrix());
+        lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
+        lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
+
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
+        let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        WRevoluteVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            ii1_sqrt,
+            basis1,
+            im2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            inv_lhs,
+            rhs,
+            r1,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(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<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        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.basis1.tr_mul(&(ang_vel2 - 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_impulse = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(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::RevoluteJoint(rev) = &mut joint.params {
+                rev.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WRevoluteVelocityGroundConstraint {
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    r2: Vector<SimdFloat>,
+
+    inv_lhs: Matrix5<SimdFloat>,
+    rhs: Vector5<SimdFloat>,
+    impulse: Vector5<SimdFloat>,
+
+    basis1: Matrix3x2<SimdFloat>,
+
+    im2: SimdFloat,
+
+    ii2_sqrt: AngularInertia<SimdFloat>,
+}
+
+impl WRevoluteVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&RevoluteJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let local_anchor1 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+        );
+        let local_anchor2 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+        );
+        let basis1 = Matrix3x2::from_columns(&[
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
+                ),
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
+                ),
+        ]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+
+        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //            .unwrap_or(Rotation::identity())
+        //            .to_rotation_matrix()
+        //            .into_inner();
+        //        let basis2 = /*r21 * */ basis1;
+        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 = basis1.tr_mul(&(ii2 * r2_mat));
+        let lhs11 = ii2.quadform3x2(&basis1).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::<U3, U3>(0, 0)
+            .copy_from(&lhs00.into_matrix());
+        lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
+        lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
+
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
+        let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        WRevoluteVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2,
+            im2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            basis1,
+            inv_lhs,
+            rhs,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(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<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        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.basis1.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::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::RevoluteJoint(rev) = &mut joint.params {
+                rev.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}

src/dynamics/solver/mod.rs

@@ -0,0 +1,56 @@
+#[cfg(not(feature = "parallel"))]
+pub(crate) use self::island_solver::IslandSolver;
+#[cfg(feature = "parallel")]
+pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext};
+#[cfg(feature = "parallel")]
+pub(self) use self::parallel_position_solver::ParallelPositionSolver;
+#[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::velocity_solver::VelocitySolver;
+pub(self) use delta_vel::DeltaVel;
+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(self) use velocity_constraint::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use velocity_constraint_wide::*;
+pub(self) use velocity_ground_constraint::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use velocity_ground_constraint_wide::*;
+
+mod categorization;
+mod delta_vel;
+mod interaction_groups;
+#[cfg(not(feature = "parallel"))]
+mod island_solver;
+mod joint_constraint;
+#[cfg(feature = "parallel")]
+mod parallel_island_solver;
+#[cfg(feature = "parallel")]
+mod parallel_position_solver;
+#[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;
+mod velocity_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod velocity_constraint_wide;
+mod velocity_ground_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod velocity_ground_constraint_wide;
+#[cfg(not(feature = "parallel"))]
+mod velocity_solver;

src/dynamics/solver/parallel_island_solver.rs

@@ -0,0 +1,259 @@
+use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
+use crate::dynamics::solver::ParallelPositionSolver;
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::Isometry;
+use crate::utils::WAngularInertia;
+use rayon::Scope;
+use std::sync::atomic::{AtomicUsize, Ordering};
+
+#[macro_export]
+#[doc(hidden)]
+macro_rules! concurrent_loop {
+    (let batch_size = $batch_size: expr;
+     for $elt: ident in $array: ident[$index_stream:expr,$index_count:expr] $f: expr) => {
+        let max_index = $array.len();
+
+        if max_index > 0 {
+            loop {
+                let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst);
+                if start_index > max_index {
+                    break;
+                }
+
+                let end_index = (start_index + $batch_size).min(max_index);
+                for $elt in &$array[start_index..end_index] {
+                    $f
+                }
+
+                $index_count.fetch_add(end_index - start_index, Ordering::SeqCst);
+            }
+        }
+    };
+
+    (let batch_size = $batch_size: expr;
+     for $elt: ident in $array: ident[$index_stream:expr] $f: expr) => {
+        let max_index = $array.len();
+
+        if max_index > 0 {
+            loop {
+                let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst);
+                if start_index > max_index {
+                    break;
+                }
+
+                let end_index = (start_index + $batch_size).min(max_index);
+                for $elt in &$array[start_index..end_index] {
+                    $f
+                }
+            }
+        }
+    };
+}
+
+pub(crate) struct ThreadContext {
+    pub batch_size: usize,
+    // Velocity solver.
+    pub constraint_initialization_index: AtomicUsize,
+    pub num_initialized_constraints: AtomicUsize,
+    pub joint_constraint_initialization_index: AtomicUsize,
+    pub num_initialized_joint_constraints: AtomicUsize,
+    pub warmstart_contact_index: AtomicUsize,
+    pub num_warmstarted_contacts: AtomicUsize,
+    pub warmstart_joint_index: AtomicUsize,
+    pub num_warmstarted_joints: AtomicUsize,
+    pub solve_interaction_index: AtomicUsize,
+    pub num_solved_interactions: AtomicUsize,
+    pub impulse_writeback_index: AtomicUsize,
+    pub joint_writeback_index: AtomicUsize,
+    pub body_integration_index: AtomicUsize,
+    pub num_integrated_bodies: AtomicUsize,
+    // Position solver.
+    pub position_constraint_initialization_index: AtomicUsize,
+    pub num_initialized_position_constraints: AtomicUsize,
+    pub position_joint_constraint_initialization_index: AtomicUsize,
+    pub num_initialized_position_joint_constraints: AtomicUsize,
+    pub solve_position_interaction_index: AtomicUsize,
+    pub num_solved_position_interactions: AtomicUsize,
+    pub position_writeback_index: AtomicUsize,
+}
+
+impl ThreadContext {
+    pub fn new(batch_size: usize) -> Self {
+        ThreadContext {
+            batch_size, // TODO perhaps there is some optimal value we can compute depending on the island size?
+            constraint_initialization_index: AtomicUsize::new(0),
+            num_initialized_constraints: AtomicUsize::new(0),
+            joint_constraint_initialization_index: AtomicUsize::new(0),
+            num_initialized_joint_constraints: AtomicUsize::new(0),
+            num_warmstarted_contacts: AtomicUsize::new(0),
+            warmstart_contact_index: AtomicUsize::new(0),
+            num_warmstarted_joints: AtomicUsize::new(0),
+            warmstart_joint_index: AtomicUsize::new(0),
+            solve_interaction_index: AtomicUsize::new(0),
+            num_solved_interactions: AtomicUsize::new(0),
+            impulse_writeback_index: AtomicUsize::new(0),
+            joint_writeback_index: AtomicUsize::new(0),
+            body_integration_index: AtomicUsize::new(0),
+            num_integrated_bodies: AtomicUsize::new(0),
+            position_constraint_initialization_index: AtomicUsize::new(0),
+            num_initialized_position_constraints: AtomicUsize::new(0),
+            position_joint_constraint_initialization_index: AtomicUsize::new(0),
+            num_initialized_position_joint_constraints: AtomicUsize::new(0),
+            solve_position_interaction_index: AtomicUsize::new(0),
+            num_solved_position_interactions: AtomicUsize::new(0),
+            position_writeback_index: AtomicUsize::new(0),
+        }
+    }
+
+    pub fn lock_until_ge(val: &AtomicUsize, target: usize) {
+        if target > 0 {
+            //        let backoff = crossbeam::utils::Backoff::new();
+            std::sync::atomic::fence(Ordering::SeqCst);
+            while val.load(Ordering::Relaxed) < target {
+                //  backoff.spin();
+                // std::thread::yield_now();
+            }
+        }
+    }
+}
+
+pub struct ParallelIslandSolver {
+    mj_lambdas: Vec<DeltaVel<f32>>,
+    positions: Vec<Isometry<f32>>,
+    parallel_groups: ParallelInteractionGroups,
+    parallel_joint_groups: ParallelInteractionGroups,
+    parallel_velocity_solver: ParallelVelocitySolver,
+    parallel_position_solver: ParallelPositionSolver,
+    thread: ThreadContext,
+}
+
+impl ParallelIslandSolver {
+    pub fn new() -> Self {
+        Self {
+            mj_lambdas: Vec::new(),
+            positions: Vec::new(),
+            parallel_groups: ParallelInteractionGroups::new(),
+            parallel_joint_groups: ParallelInteractionGroups::new(),
+            parallel_velocity_solver: ParallelVelocitySolver::new(),
+            parallel_position_solver: ParallelPositionSolver::new(),
+            thread: ThreadContext::new(8),
+        }
+    }
+
+    pub fn solve_island<'s>(
+        &'s mut self,
+        scope: &Scope<'s>,
+        island_id: usize,
+        params: &'s IntegrationParameters,
+        bodies: &'s mut RigidBodySet,
+        manifolds: &'s mut Vec<&'s mut ContactManifold>,
+        manifold_indices: &'s [ContactManifoldIndex],
+        joints: &'s mut Vec<JointGraphEdge>,
+        joint_indices: &[JointIndex],
+    ) {
+        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.parallel_groups
+            .group_interactions(island_id, bodies, manifolds, manifold_indices);
+        self.parallel_joint_groups
+            .group_interactions(island_id, bodies, joints, joint_indices);
+        self.parallel_velocity_solver.init_constraint_groups(
+            island_id,
+            bodies,
+            manifolds,
+            &self.parallel_groups,
+            joints,
+            &self.parallel_joint_groups,
+        );
+        self.parallel_position_solver.init_constraint_groups(
+            island_id,
+            bodies,
+            manifolds,
+            &self.parallel_groups,
+            joints,
+            &self.parallel_joint_groups,
+        );
+
+        self.mj_lambdas.clear();
+        self.mj_lambdas
+            .resize(bodies.active_island(island_id).len(), DeltaVel::zero());
+        self.positions.clear();
+        self.positions
+            .resize(bodies.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 positions = std::sync::atomic::AtomicPtr::new(&mut self.positions 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 parallel_velocity_solver =
+                std::sync::atomic::AtomicPtr::new(&mut self.parallel_velocity_solver as *mut _);
+            let parallel_position_solver =
+                std::sync::atomic::AtomicPtr::new(&mut self.parallel_position_solver as *mut _);
+
+            scope.spawn(move |_| {
+                // Transmute *mut -> &mut
+                let mj_lambdas: &mut Vec<DeltaVel<f32>> =
+                    unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
+                let positions: &mut Vec<Isometry<f32>> =
+                    unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
+                let bodies: &mut RigidBodySet =
+                    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_velocity_solver: &mut ParallelVelocitySolver = unsafe {
+                    std::mem::transmute(parallel_velocity_solver.load(Ordering::Relaxed))
+                };
+                let parallel_position_solver: &mut ParallelPositionSolver = unsafe {
+                    std::mem::transmute(parallel_position_solver.load(Ordering::Relaxed))
+                };
+
+                enable_flush_to_zero!(); // Ensure this is enabled on each thread.
+                parallel_velocity_solver.fill_constraints(&thread, params, bodies, manifolds, joints);
+                parallel_position_solver.fill_constraints(&thread, params, bodies, manifolds, joints);
+                parallel_velocity_solver
+                    .solve_constraints(&thread, params, manifolds, joints, mj_lambdas);
+
+                // Write results back to rigid bodies and integrate velocities.
+                let island_range = bodies.active_island_range(island_id);
+                let active_bodies = &bodies.active_dynamic_set[island_range];
+                let bodies = &mut bodies.bodies;
+
+                concurrent_loop! {
+                    let batch_size = thread.batch_size;
+                    for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
+                        let rb = &mut bodies[*handle];
+                        let dvel = mj_lambdas[rb.active_set_offset];
+                        rb.linvel += dvel.linear;
+                        rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular);
+                        rb.integrate(params.dt());
+                        positions[rb.active_set_offset] = rb.position;
+                    }
+                }
+
+                // We need to way for every body to be integrated because it also
+                // initialized `positions` to the updated values.
+                ThreadContext::lock_until_ge(&thread.num_integrated_bodies, active_bodies.len());
+
+                parallel_position_solver.solve_constraints(&thread, params, positions);
+
+                // 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 = &mut bodies[*handle];
+                        rb.set_position(positions[rb.active_set_offset]);
+                    }
+                }
+            })
+        }
+    }
+}

src/dynamics/solver/parallel_position_solver.rs

@@ -0,0 +1,582 @@
+use super::ParallelInteractionGroups;
+use super::{AnyJointPositionConstraint, AnyPositionConstraint, ThreadContext};
+use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts};
+use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint};
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
+use crate::geometry::ContactManifold;
+use crate::math::Isometry;
+#[cfg(feature = "simd-is-enabled")]
+use crate::{
+    dynamics::solver::{WPositionConstraint, WPositionGroundConstraint},
+    simd::SIMD_WIDTH,
+};
+use std::sync::atomic::Ordering;
+
+pub(crate) enum PositionConstraintDesc {
+    NongroundNongrouped(usize),
+    GroundNongrouped(usize),
+    #[cfg(feature = "simd-is-enabled")]
+    NongroundGrouped([usize; SIMD_WIDTH]),
+    #[cfg(feature = "simd-is-enabled")]
+    GroundGrouped([usize; SIMD_WIDTH]),
+}
+
+pub(crate) struct ParallelPositionSolverContactPart {
+    pub point_point: Vec<usize>,
+    pub plane_point: Vec<usize>,
+    pub ground_point_point: Vec<usize>,
+    pub ground_plane_point: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+    pub ground_interaction_groups: InteractionGroups,
+    pub constraints: Vec<AnyPositionConstraint>,
+    pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
+    pub parallel_desc_groups: Vec<usize>,
+}
+
+pub(crate) struct ParallelPositionSolverJointPart {
+    pub not_ground_interactions: Vec<usize>,
+    pub ground_interactions: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+    pub ground_interaction_groups: InteractionGroups,
+    pub constraints: Vec<AnyJointPositionConstraint>,
+    pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
+    pub parallel_desc_groups: Vec<usize>,
+}
+
+impl ParallelPositionSolverContactPart {
+    pub fn new() -> Self {
+        Self {
+            point_point: Vec::new(),
+            plane_point: Vec::new(),
+            ground_point_point: Vec::new(),
+            ground_plane_point: Vec::new(),
+            interaction_groups: InteractionGroups::new(),
+            ground_interaction_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+            constraint_descs: Vec::new(),
+            parallel_desc_groups: Vec::new(),
+        }
+    }
+}
+impl ParallelPositionSolverJointPart {
+    pub fn new() -> Self {
+        Self {
+            not_ground_interactions: Vec::new(),
+            ground_interactions: Vec::new(),
+            interaction_groups: InteractionGroups::new(),
+            ground_interaction_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+            constraint_descs: Vec::new(),
+            parallel_desc_groups: Vec::new(),
+        }
+    }
+}
+
+impl ParallelPositionSolverJointPart {
+    pub fn init_constraints_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        joints: &mut [JointGraphEdge],
+        joint_groups: &ParallelInteractionGroups,
+    ) {
+        let mut total_num_constraints = 0;
+        let num_groups = joint_groups.num_groups();
+
+        self.interaction_groups.clear_groups();
+        self.ground_interaction_groups.clear_groups();
+        self.parallel_desc_groups.clear();
+        self.constraint_descs.clear();
+        self.parallel_desc_groups.push(0);
+
+        for i in 0..num_groups {
+            let group = joint_groups.group(i);
+
+            self.not_ground_interactions.clear();
+            self.ground_interactions.clear();
+            categorize_joints(
+                bodies,
+                joints,
+                group,
+                &mut self.ground_interactions,
+                &mut self.not_ground_interactions,
+            );
+
+            #[cfg(feature = "simd-is-enabled")]
+            let start_grouped = self.interaction_groups.grouped_interactions.len();
+            let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
+            #[cfg(feature = "simd-is-enabled")]
+            let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
+            let start_nongrouped_ground =
+                self.ground_interaction_groups.nongrouped_interactions.len();
+
+            self.interaction_groups.group_joints(
+                island_id,
+                bodies,
+                joints,
+                &self.not_ground_interactions,
+            );
+            self.ground_interaction_groups.group_joints(
+                island_id,
+                bodies,
+                joints,
+                &self.ground_interactions,
+            );
+
+            // Compute constraint indices.
+            for interaction_i in
+                &self.interaction_groups.nongrouped_interactions[start_nongrouped..]
+            {
+                let joint = &mut joints[*interaction_i].weight;
+                joint.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::NongroundNongrouped(*interaction_i),
+                ));
+                total_num_constraints +=
+                    AnyJointPositionConstraint::num_active_constraints(joint, false);
+            }
+
+            #[cfg(feature = "simd-is-enabled")]
+            for interaction_i in
+                self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
+            {
+                let joint = &mut joints[interaction_i[0]].weight;
+                joint.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::NongroundGrouped(
+                        array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                    ),
+                ));
+                total_num_constraints +=
+                    AnyJointPositionConstraint::num_active_constraints(joint, true);
+            }
+
+            for interaction_i in
+                &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
+            {
+                let joint = &mut joints[*interaction_i].weight;
+                joint.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::GroundNongrouped(*interaction_i),
+                ));
+                total_num_constraints +=
+                    AnyJointPositionConstraint::num_active_constraints(joint, false);
+            }
+
+            #[cfg(feature = "simd-is-enabled")]
+            for interaction_i in self.ground_interaction_groups.grouped_interactions
+                [start_grouped_ground..]
+                .chunks(SIMD_WIDTH)
+            {
+                let joint = &mut joints[interaction_i[0]].weight;
+                joint.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::GroundGrouped(
+                        array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                    ),
+                ));
+                total_num_constraints +=
+                    AnyJointPositionConstraint::num_active_constraints(joint, true);
+            }
+
+            self.parallel_desc_groups.push(self.constraint_descs.len());
+        }
+
+        // Resize the constraints set.
+        self.constraints.clear();
+        self.constraints
+            .resize_with(total_num_constraints, || AnyJointPositionConstraint::Empty)
+    }
+
+    fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        let descs = &self.constraint_descs;
+
+        crate::concurrent_loop! {
+            let batch_size = thread.batch_size;
+            for desc in descs[thread.position_joint_constraint_initialization_index, thread.num_initialized_position_joint_constraints] {
+                match &desc.1 {
+                    PositionConstraintDesc::NongroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let constraint = AnyJointPositionConstraint::from_joint(
+                            joint,
+                            bodies,
+                        );
+                        self.constraints[joint.position_constraint_index] = constraint;
+                    }
+                    PositionConstraintDesc::GroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let constraint = AnyJointPositionConstraint::from_joint_ground(
+                            joint,
+                            bodies,
+                        );
+                        self.constraints[joint.position_constraint_index] = constraint;
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    PositionConstraintDesc::NongroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint(
+                            joints, bodies,
+                        ) {
+                            self.constraints[joints[0].position_constraint_index] = constraint
+                        } else {
+                            for ii in 0..SIMD_WIDTH {
+                                let constraint = AnyJointPositionConstraint::from_joint(joints[ii], bodies);
+                                self.constraints[joints[0].position_constraint_index + ii] = constraint;
+                            }
+                        }
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    PositionConstraintDesc::GroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint_ground(
+                            joints, bodies,
+                        ) {
+                            self.constraints[joints[0].position_constraint_index] = constraint
+                        } else {
+                            for ii in 0..SIMD_WIDTH {
+                                let constraint = AnyJointPositionConstraint::from_joint_ground(joints[ii], bodies);
+                                self.constraints[joints[0].position_constraint_index + ii] = constraint;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+impl ParallelPositionSolverContactPart {
+    pub fn init_constraints_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        manifolds: &mut [&mut ContactManifold],
+        manifold_groups: &ParallelInteractionGroups,
+    ) {
+        let mut total_num_constraints = 0;
+        let num_groups = manifold_groups.num_groups();
+
+        self.interaction_groups.clear_groups();
+        self.ground_interaction_groups.clear_groups();
+        self.parallel_desc_groups.clear();
+        self.constraint_descs.clear();
+        self.parallel_desc_groups.push(0);
+
+        for i in 0..num_groups {
+            let group = manifold_groups.group(i);
+
+            self.ground_point_point.clear();
+            self.ground_plane_point.clear();
+            self.point_point.clear();
+            self.plane_point.clear();
+            categorize_position_contacts(
+                bodies,
+                manifolds,
+                group,
+                &mut self.ground_point_point,
+                &mut self.ground_plane_point,
+                &mut self.point_point,
+                &mut self.plane_point,
+            );
+
+            #[cfg(feature = "simd-is-enabled")]
+            let start_grouped = self.interaction_groups.grouped_interactions.len();
+            let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
+            #[cfg(feature = "simd-is-enabled")]
+            let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
+            let start_nongrouped_ground =
+                self.ground_interaction_groups.nongrouped_interactions.len();
+
+            self.interaction_groups.group_manifolds(
+                island_id,
+                bodies,
+                manifolds,
+                &self.point_point,
+            );
+            self.interaction_groups.group_manifolds(
+                island_id,
+                bodies,
+                manifolds,
+                &self.plane_point,
+            );
+            self.ground_interaction_groups.group_manifolds(
+                island_id,
+                bodies,
+                manifolds,
+                &self.ground_point_point,
+            );
+            self.ground_interaction_groups.group_manifolds(
+                island_id,
+                bodies,
+                manifolds,
+                &self.ground_plane_point,
+            );
+
+            // Compute constraint indices.
+            for interaction_i in
+                &self.interaction_groups.nongrouped_interactions[start_nongrouped..]
+            {
+                let manifold = &mut *manifolds[*interaction_i];
+                manifold.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::NongroundNongrouped(*interaction_i),
+                ));
+                total_num_constraints += PositionConstraint::num_active_constraints(manifold);
+            }
+
+            #[cfg(feature = "simd-is-enabled")]
+            for interaction_i in
+                self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
+            {
+                let manifold = &mut *manifolds[interaction_i[0]];
+                manifold.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::NongroundGrouped(
+                        array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                    ),
+                ));
+                total_num_constraints += PositionConstraint::num_active_constraints(manifold);
+            }
+
+            for interaction_i in
+                &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
+            {
+                let manifold = &mut *manifolds[*interaction_i];
+                manifold.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::GroundNongrouped(*interaction_i),
+                ));
+                total_num_constraints += PositionConstraint::num_active_constraints(manifold);
+            }
+
+            #[cfg(feature = "simd-is-enabled")]
+            for interaction_i in self.ground_interaction_groups.grouped_interactions
+                [start_grouped_ground..]
+                .chunks(SIMD_WIDTH)
+            {
+                let manifold = &mut *manifolds[interaction_i[0]];
+                manifold.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::GroundGrouped(
+                        array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                    ),
+                ));
+                total_num_constraints += PositionConstraint::num_active_constraints(manifold);
+            }
+
+            self.parallel_desc_groups.push(self.constraint_descs.len());
+        }
+
+        // Resize the constraints set.
+        self.constraints.clear();
+        self.constraints
+            .resize_with(total_num_constraints, || AnyPositionConstraint::Empty)
+    }
+
+    fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        let descs = &self.constraint_descs;
+
+        crate::concurrent_loop! {
+            let batch_size = thread.batch_size;
+            for desc in descs[thread.position_constraint_initialization_index, thread.num_initialized_position_constraints] {
+                match &desc.1 {
+                    PositionConstraintDesc::NongroundNongrouped(manifold_id) => {
+                        let manifold = &*manifolds_all[*manifold_id];
+                        PositionConstraint::generate(
+                            params,
+                            manifold,
+                            bodies,
+                            &mut self.constraints,
+                            false,
+                        );
+                    }
+                    PositionConstraintDesc::GroundNongrouped(manifold_id) => {
+                        let manifold = &*manifolds_all[*manifold_id];
+                        PositionGroundConstraint::generate(
+                            params,
+                            manifold,
+                            bodies,
+                            &mut self.constraints,
+                            false,
+                        );
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    PositionConstraintDesc::NongroundGrouped(manifold_id) => {
+                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        WPositionConstraint::generate(
+                            params,
+                            manifolds,
+                            bodies,
+                            &mut self.constraints,
+                            false,
+                        );
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    PositionConstraintDesc::GroundGrouped(manifold_id) => {
+                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        WPositionGroundConstraint::generate(
+                            params,
+                            manifolds,
+                            bodies,
+                            &mut self.constraints,
+                            false,
+                        );
+                    }
+                }
+            }
+        }
+    }
+}
+
+pub(crate) struct ParallelPositionSolver {
+    part: ParallelPositionSolverContactPart,
+    joint_part: ParallelPositionSolverJointPart,
+}
+
+impl ParallelPositionSolver {
+    pub fn new() -> Self {
+        Self {
+            part: ParallelPositionSolverContactPart::new(),
+            joint_part: ParallelPositionSolverJointPart::new(),
+        }
+    }
+
+    pub fn init_constraint_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        manifolds: &mut [&mut ContactManifold],
+        manifold_groups: &ParallelInteractionGroups,
+        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);
+    }
+
+    pub fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        joints: &[JointGraphEdge],
+    ) {
+        self.part
+            .fill_constraints(thread, params, bodies, manifolds);
+        self.joint_part.fill_constraints(thread, bodies, joints);
+        ThreadContext::lock_until_ge(
+            &thread.num_initialized_position_constraints,
+            self.part.constraint_descs.len(),
+        );
+        ThreadContext::lock_until_ge(
+            &thread.num_initialized_position_joint_constraints,
+            self.joint_part.constraint_descs.len(),
+        );
+    }
+
+    pub fn solve_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        if self.part.constraint_descs.len() == 0 {
+            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 = &self.part.constraint_descs[..];
+            let joint_descs = &self.joint_part.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.constraints[$part.constraint_descs[start_index].0..]
+                                } else {
+                                    &mut $part.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!(self.joint_part);
+                shift += joint_descs.len();
+                start_index -= joint_descs.len();
+                solve!(self.part);
+                shift += contact_descs.len();
+                start_index -= contact_descs.len();
+            }
+        }
+    }
+}

src/dynamics/solver/parallel_velocity_solver.rs

@@ -0,0 +1,485 @@
+use super::ParallelInteractionGroups;
+use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
+use crate::dynamics::solver::categorization::{categorize_joints, categorize_velocity_contacts};
+use crate::dynamics::solver::{InteractionGroups, VelocityConstraint, VelocityGroundConstraint};
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
+use crate::geometry::ContactManifold;
+#[cfg(feature = "simd-is-enabled")]
+use crate::{
+    dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
+    simd::SIMD_WIDTH,
+};
+use std::sync::atomic::Ordering;
+
+pub(crate) enum VelocityConstraintDesc {
+    NongroundNongrouped(usize),
+    GroundNongrouped(usize),
+    #[cfg(feature = "simd-is-enabled")]
+    NongroundGrouped([usize; SIMD_WIDTH]),
+    #[cfg(feature = "simd-is-enabled")]
+    GroundGrouped([usize; SIMD_WIDTH]),
+}
+
+pub(crate) struct ParallelVelocitySolverPart<Constraint> {
+    pub not_ground_interactions: Vec<usize>,
+    pub ground_interactions: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+    pub ground_interaction_groups: InteractionGroups,
+    pub constraints: Vec<Constraint>,
+    pub constraint_descs: Vec<(usize, VelocityConstraintDesc)>,
+    pub parallel_desc_groups: Vec<usize>,
+}
+
+impl<Constraint> ParallelVelocitySolverPart<Constraint> {
+    pub fn new() -> Self {
+        Self {
+            not_ground_interactions: Vec::new(),
+            ground_interactions: Vec::new(),
+            interaction_groups: InteractionGroups::new(),
+            ground_interaction_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+            constraint_descs: Vec::new(),
+            parallel_desc_groups: Vec::new(),
+        }
+    }
+}
+
+macro_rules! impl_init_constraints_group {
+    ($Constraint: ty, $Interaction: ty, $categorize: ident, $group: ident, $num_active_constraints: path, $empty_constraint: expr $(, $weight: ident)*) => {
+        impl ParallelVelocitySolverPart<$Constraint> {
+            pub fn init_constraints_groups(
+                &mut self,
+                island_id: usize,
+                bodies: &RigidBodySet,
+                interactions: &mut [$Interaction],
+                interaction_groups: &ParallelInteractionGroups,
+            ) {
+                let mut total_num_constraints = 0;
+                let num_groups = interaction_groups.num_groups();
+
+                self.interaction_groups.clear_groups();
+                self.ground_interaction_groups.clear_groups();
+                self.parallel_desc_groups.clear();
+                self.constraint_descs.clear();
+                self.parallel_desc_groups.push(0);
+
+                for i in 0..num_groups {
+                    let group = interaction_groups.group(i);
+
+                    self.not_ground_interactions.clear();
+                    self.ground_interactions.clear();
+                    $categorize(
+                        bodies,
+                        interactions,
+                        group,
+                        &mut self.ground_interactions,
+                        &mut self.not_ground_interactions,
+                    );
+
+                    #[cfg(feature = "simd-is-enabled")]
+                    let start_grouped = self.interaction_groups.grouped_interactions.len();
+                    let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
+                    #[cfg(feature = "simd-is-enabled")]
+                    let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
+                    let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len();
+
+                    self.interaction_groups.$group(
+                        island_id,
+                        bodies,
+                        interactions,
+                        &self.not_ground_interactions,
+                    );
+                    self.ground_interaction_groups.$group(
+                        island_id,
+                        bodies,
+                        interactions,
+                        &self.ground_interactions,
+                    );
+
+                    // Compute constraint indices.
+                    for interaction_i in &self.interaction_groups.nongrouped_interactions[start_nongrouped..] {
+                        let interaction = &mut interactions[*interaction_i]$(.$weight)*;
+                        interaction.constraint_index = total_num_constraints;
+                        self.constraint_descs.push((
+                            total_num_constraints,
+                            VelocityConstraintDesc::NongroundNongrouped(*interaction_i),
+                        ));
+                        total_num_constraints += $num_active_constraints(interaction);
+                    }
+
+                    #[cfg(feature = "simd-is-enabled")]
+                    for interaction_i in
+                        self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
+                    {
+                        let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
+                        interaction.constraint_index = total_num_constraints;
+                        self.constraint_descs.push((
+                            total_num_constraints,
+                            VelocityConstraintDesc::NongroundGrouped(
+                                array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                            ),
+                        ));
+                        total_num_constraints += $num_active_constraints(interaction);
+                    }
+
+                    for interaction_i in
+                        &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
+                    {
+                        let interaction = &mut interactions[*interaction_i]$(.$weight)*;
+                        interaction.constraint_index = total_num_constraints;
+                        self.constraint_descs.push((
+                            total_num_constraints,
+                            VelocityConstraintDesc::GroundNongrouped(*interaction_i),
+                        ));
+                        total_num_constraints += $num_active_constraints(interaction);
+                    }
+
+                    #[cfg(feature = "simd-is-enabled")]
+                    for interaction_i in self.ground_interaction_groups.grouped_interactions
+                        [start_grouped_ground..]
+                        .chunks(SIMD_WIDTH)
+                    {
+                        let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
+                        interaction.constraint_index = total_num_constraints;
+                        self.constraint_descs.push((
+                            total_num_constraints,
+                            VelocityConstraintDesc::GroundGrouped(
+                                array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                            ),
+                        ));
+                        total_num_constraints += $num_active_constraints(interaction);
+                    }
+
+                    self.parallel_desc_groups.push(self.constraint_descs.len());
+                }
+
+                // Resize the constraints set.
+                self.constraints.clear();
+                self.constraints
+                    .resize_with(total_num_constraints, || $empty_constraint)
+            }
+        }
+    }
+}
+
+impl_init_constraints_group!(
+    AnyVelocityConstraint,
+    &mut ContactManifold,
+    categorize_velocity_contacts,
+    group_manifolds,
+    VelocityConstraint::num_active_constraints,
+    AnyVelocityConstraint::Empty
+);
+
+impl_init_constraints_group!(
+    AnyJointVelocityConstraint,
+    JointGraphEdge,
+    categorize_joints,
+    group_joints,
+    AnyJointVelocityConstraint::num_active_constraints,
+    AnyJointVelocityConstraint::Empty,
+    weight
+);
+
+impl ParallelVelocitySolverPart<AnyVelocityConstraint> {
+    fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        let descs = &self.constraint_descs;
+
+        crate::concurrent_loop! {
+            let batch_size = thread.batch_size;
+            for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] {
+                match &desc.1 {
+                    VelocityConstraintDesc::NongroundNongrouped(manifold_id) => {
+                        let manifold = &*manifolds_all[*manifold_id];
+                        VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
+                    }
+                    VelocityConstraintDesc::GroundNongrouped(manifold_id) => {
+                        let manifold = &*manifolds_all[*manifold_id];
+                        VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    VelocityConstraintDesc::NongroundGrouped(manifold_id) => {
+                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    VelocityConstraintDesc::GroundGrouped(manifold_id) => {
+                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
+                    }
+                }
+            }
+        }
+    }
+}
+
+impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
+    fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        let descs = &self.constraint_descs;
+
+        crate::concurrent_loop! {
+            let batch_size = thread.batch_size;
+            for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] {
+                match &desc.1 {
+                    VelocityConstraintDesc::NongroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let constraint = AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies);
+                        self.constraints[joint.constraint_index] = constraint;
+                    }
+                    VelocityConstraintDesc::GroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
+                        self.constraints[joint.constraint_index] = constraint;
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    VelocityConstraintDesc::NongroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
+                        self.constraints[joints[0].constraint_index] = constraint;
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    VelocityConstraintDesc::GroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
+                        self.constraints[joints[0].constraint_index] = constraint;
+                    }
+                }
+            }
+        }
+    }
+}
+
+pub(crate) struct ParallelVelocitySolver {
+    part: ParallelVelocitySolverPart<AnyVelocityConstraint>,
+    joint_part: ParallelVelocitySolverPart<AnyJointVelocityConstraint>,
+}
+
+impl ParallelVelocitySolver {
+    pub fn new() -> Self {
+        Self {
+            part: ParallelVelocitySolverPart::new(),
+            joint_part: ParallelVelocitySolverPart::new(),
+        }
+    }
+
+    pub fn init_constraint_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        manifolds: &mut [&mut ContactManifold],
+        manifold_groups: &ParallelInteractionGroups,
+        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);
+    }
+
+    pub fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        joints: &[JointGraphEdge],
+    ) {
+        self.part
+            .fill_constraints(thread, params, bodies, manifolds);
+        self.joint_part
+            .fill_constraints(thread, params, bodies, joints);
+        ThreadContext::lock_until_ge(
+            &thread.num_initialized_constraints,
+            self.part.constraint_descs.len(),
+        );
+        ThreadContext::lock_until_ge(
+            &thread.num_initialized_joint_constraints,
+            self.joint_part.constraint_descs.len(),
+        );
+    }
+
+    pub fn solve_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        manifolds_all: &mut [&mut ContactManifold],
+        joints_all: &mut [JointGraphEdge],
+        mj_lambdas: &mut [DeltaVel<f32>],
+    ) {
+        if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 {
+            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_contact_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.constraints[$part.constraint_descs[start_index].0..]
+                            } else {
+                                &mut $part.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_contacts
+                                .fetch_add(num_solved, Ordering::SeqCst);
+
+                            if batch_size == 0 {
+                                start_index = thread
+                                    .warmstart_contact_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_contacts, target_num_desc);
+                    }
+                }
+            );
+
+            warmstart!(self.joint_part);
+            shift = self.joint_part.constraint_descs.len();
+            start_index -= shift;
+            warmstart!(self.part);
+        }
+
+        /*
+         * 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 parallel_desc_group, we have to wait util the current group is finished
+            // before starting the next one.
+            let mut start_index = thread
+                .solve_interaction_index
+                .fetch_add(thread.batch_size, Ordering::SeqCst);
+            let mut batch_size = thread.batch_size;
+            let contact_descs = &self.part.constraint_descs[..];
+            let joint_descs = &self.joint_part.constraint_descs[..];
+            let mut target_num_desc = 0;
+            let mut shift = 0;
+
+            for _ in 0..params.max_velocity_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.constraints[$part.constraint_descs[start_index].0..]
+                                } else {
+                                    &mut $part.constraints[$part.constraint_descs[start_index].0
+                                        ..$part.constraint_descs[end_index].0]
+                                };
+
+                                //                                println!(
+                                //                                    "Solving a constraint {:?}.",
+                                //                                    rayon::current_thread_index()
+                                //                                );
+                                for constraint in constraints {
+                                    constraint.solve(mj_lambdas);
+                                }
+
+                                let num_solved = end_index - start_index;
+                                batch_size -= num_solved;
+
+                                thread
+                                    .num_solved_interactions
+                                    .fetch_add(num_solved, Ordering::SeqCst);
+
+                                if batch_size == 0 {
+                                    start_index = thread
+                                        .solve_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_interactions,
+                                target_num_desc,
+                            );
+                        }
+                    };
+                }
+
+                solve!(self.joint_part);
+                shift += joint_descs.len();
+                start_index -= joint_descs.len();
+                solve!(self.part);
+                shift += contact_descs.len();
+                start_index -= contact_descs.len();
+            }
+        }
+
+        /*
+         * Writeback impulses.
+         */
+        let joint_constraints = &self.joint_part.constraints;
+        let contact_constraints = &self.part.constraints;
+        crate::concurrent_loop! {
+             let batch_size = thread.batch_size;
+             for constraint in joint_constraints[thread.joint_writeback_index] {
+                 constraint.writeback_impulses(joints_all);
+             }
+        }
+        crate::concurrent_loop! {
+             let batch_size = thread.batch_size;
+             for constraint in contact_constraints[thread.impulse_writeback_index] {
+                 constraint.writeback_impulses(manifolds_all);
+             }
+        }
+    }
+}

src/dynamics/solver/position_constraint.rs

@@ -0,0 +1,246 @@
+use crate::dynamics::solver::PositionGroundConstraint;
+#[cfg(feature = "simd-is-enabled")]
+use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint};
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, KinematicsCategory};
+use crate::math::{
+    AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
+};
+use crate::utils::{WAngularInertia, WCross, WDot};
+
+pub(crate) enum AnyPositionConstraint {
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedPointPointGround(WPositionGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedPlanePointGround(WPositionGroundConstraint),
+    NongroupedPointPointGround(PositionGroundConstraint),
+    NongroupedPlanePointGround(PositionGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedPointPoint(WPositionConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedPlanePoint(WPositionConstraint),
+    NongroupedPointPoint(PositionConstraint),
+    NongroupedPlanePoint(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<f32>]) {
+        match self {
+            #[cfg(feature = "simd-is-enabled")]
+            AnyPositionConstraint::GroupedPointPointGround(c) => {
+                c.solve_point_point(params, positions)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            AnyPositionConstraint::GroupedPlanePointGround(c) => {
+                c.solve_plane_point(params, positions)
+            }
+            AnyPositionConstraint::NongroupedPointPointGround(c) => {
+                c.solve_point_point(params, positions)
+            }
+            AnyPositionConstraint::NongroupedPlanePointGround(c) => {
+                c.solve_plane_point(params, positions)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            AnyPositionConstraint::GroupedPointPoint(c) => c.solve_point_point(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyPositionConstraint::GroupedPlanePoint(c) => c.solve_plane_point(params, positions),
+            AnyPositionConstraint::NongroupedPointPoint(c) => {
+                c.solve_point_point(params, positions)
+            }
+            AnyPositionConstraint::NongroupedPlanePoint(c) => {
+                c.solve_plane_point(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<f32>; MAX_MANIFOLD_POINTS],
+    pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
+    pub local_n1: Vector<f32>,
+    pub num_contacts: u8,
+    pub radius: f32,
+    pub im1: f32,
+    pub im2: f32,
+    pub ii1: AngularInertia<f32>,
+    pub ii2: AngularInertia<f32>,
+    pub erp: f32,
+    pub max_linear_correction: f32,
+}
+
+impl PositionConstraint {
+    #[cfg(feature = "parallel")]
+    pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
+        let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
+        manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
+    }
+
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyPositionConstraint>,
+        push: bool,
+    ) {
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+        let shift1 = manifold.local_n1 * -manifold.kinematics.radius1;
+        let shift2 = manifold.local_n2 * -manifold.kinematics.radius2;
+        let radius =
+            manifold.kinematics.radius1 + manifold.kinematics.radius2 /*- params.allowed_linear_error*/;
+
+        for (l, manifold_points) in manifold
+            .active_contacts()
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
+            let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
+
+            for l in 0..manifold_points.len() {
+                local_p1[l] = manifold_points[l].local_p1 + shift1;
+                local_p2[l] = manifold_points[l].local_p2 + shift2;
+            }
+
+            let constraint = PositionConstraint {
+                rb1: rb1.active_set_offset,
+                rb2: rb2.active_set_offset,
+                local_p1,
+                local_p2,
+                local_n1: manifold.local_n1,
+                radius,
+                im1: rb1.mass_properties.inv_mass,
+                im2: rb2.mass_properties.inv_mass,
+                ii1: rb1.world_inv_inertia_sqrt.squared(),
+                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                num_contacts: manifold_points.len() as u8,
+                erp: params.erp,
+                max_linear_correction: params.max_linear_correction,
+            };
+
+            if push {
+                if manifold.kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints.push(AnyPositionConstraint::NongroupedPointPoint(constraint));
+                } else {
+                    out_constraints.push(AnyPositionConstraint::NongroupedPlanePoint(constraint));
+                }
+            } else {
+                if manifold.kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints[manifold.constraint_index + l] =
+                        AnyPositionConstraint::NongroupedPointPoint(constraint);
+                } else {
+                    out_constraints[manifold.constraint_index + l] =
+                        AnyPositionConstraint::NongroupedPlanePoint(constraint);
+                }
+            }
+        }
+    }
+
+    pub fn solve_point_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // 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;
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let p1 = pos1 * self.local_p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+
+            let sqdist = dpos.norm_squared();
+
+            // NOTE: only works for the point-point case.
+            if sqdist < target_dist * target_dist {
+                let dist = sqdist.sqrt();
+                let n = dpos / 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(n);
+                let gcross2 = -dp2.gcross(n);
+                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(n * (impulse * self.im1));
+                let tra2 = Translation::from(n * (-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;
+    }
+
+    pub fn solve_plane_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // 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;
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let n1 = pos1 * self.local_n1;
+            let p1 = pos1 * self.local_p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+            let dist = dpos.dot(&n1);
+
+            if dist < target_dist {
+                let p1 = p2 - n1 * dist;
+                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
+                let dp1 = p1.coords - pos1.translation.vector;
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross1 = dp1.gcross(n1);
+                let gcross2 = -dp2.gcross(n1);
+                let ii_gcross1 = self.ii1.transform_vector(gcross1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r =
+                    self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                let tra1 = Translation::from(n1 * (impulse * self.im1));
+                let tra2 = Translation::from(n1 * (-impulse * self.im2));
+                let rot1 = Rotation::new(ii_gcross1 * impulse);
+                let rot2 = Rotation::new(ii_gcross2 * impulse);
+
+                pos1 = Isometry::from_parts(tra1 * pos1.translation, rot1 * pos1.rotation);
+                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
+            }
+        }
+
+        positions[self.rb1] = pos1;
+        positions[self.rb2] = pos2;
+    }
+}

src/dynamics/solver/position_constraint_wide.rs

@@ -0,0 +1,217 @@
+use super::AnyPositionConstraint;
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, KinematicsCategory};
+use crate::math::{
+    AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
+    SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WDot};
+
+use num::Zero;
+use simba::simd::{SimdBool as _, SimdComplexField, 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<SimdFloat>; MAX_MANIFOLD_POINTS],
+    pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
+    pub local_n1: Vector<SimdFloat>,
+    pub radius: SimdFloat,
+    pub im1: SimdFloat,
+    pub im2: SimdFloat,
+    pub ii1: AngularInertia<SimdFloat>,
+    pub ii2: AngularInertia<SimdFloat>,
+    pub erp: SimdFloat,
+    pub max_linear_correction: SimdFloat,
+    pub num_contacts: u8,
+}
+
+impl WPositionConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifolds: [&ContactManifold; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyPositionConstraint>,
+        push: bool,
+    ) {
+        let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
+        let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
+
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let sqrt_ii1: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let sqrt_ii2: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+
+        let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
+        let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]);
+
+        let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
+        let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
+
+        let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
+
+        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
+            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+            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,
+                radius,
+                im1,
+                im2,
+                ii1: sqrt_ii1.squared(),
+                ii2: sqrt_ii2.squared(),
+                erp: SimdFloat::splat(params.erp),
+                max_linear_correction: SimdFloat::splat(params.max_linear_correction),
+                num_contacts: num_points as u8,
+            };
+
+            let shift1 = local_n1 * -radius1;
+            let shift2 = local_n2 * -radius2;
+
+            for i in 0..num_points {
+                let local_p1 =
+                    Point::from(array![|ii| manifold_points[ii][i].local_p1; SIMD_WIDTH]);
+                let local_p2 =
+                    Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]);
+
+                constraint.local_p1[i] = local_p1 + shift1;
+                constraint.local_p2[i] = local_p2 + shift2;
+            }
+
+            if push {
+                if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints.push(AnyPositionConstraint::GroupedPointPoint(constraint));
+                } else {
+                    out_constraints.push(AnyPositionConstraint::GroupedPlanePoint(constraint));
+                }
+            } else {
+                if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                        AnyPositionConstraint::GroupedPointPoint(constraint);
+                } else {
+                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                        AnyPositionConstraint::GroupedPlanePoint(constraint);
+                }
+            }
+        }
+    }
+
+    pub fn solve_point_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
+        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let allowed_err = SimdFloat::splat(params.allowed_linear_error);
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let p1 = pos1 * self.local_p1[k];
+            let p2 = pos2 * self.local_p2[k];
+
+            let dpos = p2 - p1;
+            let sqdist = dpos.norm_squared();
+
+            if sqdist.simd_lt(target_dist * target_dist).any() {
+                let dist = sqdist.simd_sqrt();
+                let n = dpos / dist;
+                let err = ((dist - target_dist) * self.erp)
+                    .simd_clamp(-self.max_linear_correction, SimdFloat::zero());
+                let dp1 = p1.coords - pos1.translation.vector;
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross1 = dp1.gcross(n);
+                let gcross2 = -dp2.gcross(n);
+                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(n * (impulse * self.im1)) * pos1.translation;
+                pos1.rotation = Rotation::new(ii_gcross1 * impulse) * pos1.rotation;
+                pos2.translation = Translation::from(n * (-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);
+        }
+    }
+
+    pub fn solve_plane_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
+        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let allowed_err = SimdFloat::splat(params.allowed_linear_error);
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            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, SimdFloat::zero());
+                let dp1 = p1.coords - pos1.translation.vector;
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross1 = dp1.gcross(n1);
+                let gcross2 = -dp2.gcross(n1);
+                let ii_gcross1 = self.ii1.transform_vector(gcross1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r =
+                    self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                pos1.translation = Translation::from(n1 * (impulse * self.im1)) * pos1.translation;
+                pos1.rotation = Rotation::new(ii_gcross1 * impulse) * pos1.rotation;
+                pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation;
+                pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
+            }
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.rb1[ii]] = pos1.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            positions[self.rb2[ii]] = pos2.extract(ii);
+        }
+    }
+}

src/dynamics/solver/position_ground_constraint.rs

@@ -0,0 +1,189 @@
+use super::AnyPositionConstraint;
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, KinematicsCategory};
+use crate::math::{
+    AngularInertia, Isometry, Point, 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<f32>; MAX_MANIFOLD_POINTS],
+    pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
+    pub n1: Vector<f32>,
+    pub num_contacts: u8,
+    pub radius: f32,
+    pub im2: f32,
+    pub ii2: AngularInertia<f32>,
+    pub erp: f32,
+    pub max_linear_correction: f32,
+}
+
+impl PositionGroundConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyPositionConstraint>,
+        push: bool,
+    ) {
+        let mut rb1 = &bodies[manifold.body_pair.body1];
+        let mut rb2 = &bodies[manifold.body_pair.body2];
+        let flip = !rb2.is_dynamic();
+
+        let local_n1;
+        let local_n2;
+
+        if flip {
+            std::mem::swap(&mut rb1, &mut rb2);
+            local_n1 = manifold.local_n2;
+            local_n2 = manifold.local_n1;
+        } else {
+            local_n1 = manifold.local_n1;
+            local_n2 = manifold.local_n2;
+        };
+
+        let shift1 = local_n1 * -manifold.kinematics.radius1;
+        let shift2 = local_n2 * -manifold.kinematics.radius2;
+        let radius =
+            manifold.kinematics.radius1 + manifold.kinematics.radius2 /* - params.allowed_linear_error */;
+
+        for (l, manifold_points) in manifold
+            .active_contacts()
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
+            let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
+
+            if flip {
+                // Don't forget that we already swapped rb1 and rb2 above.
+                // So if we flip, only manifold_points[k].{local_p1,local_p2} have to
+                // be swapped.
+                for k in 0..manifold_points.len() {
+                    p1[k] = rb1.predicted_position * (manifold_points[k].local_p2 + shift1);
+                    local_p2[k] = manifold_points[k].local_p1 + shift2;
+                }
+            } else {
+                for k in 0..manifold_points.len() {
+                    p1[k] = rb1.predicted_position * (manifold_points[k].local_p1 + shift1);
+                    local_p2[k] = manifold_points[k].local_p2 + shift2;
+                }
+            }
+
+            let constraint = PositionGroundConstraint {
+                rb2: rb2.active_set_offset,
+                p1,
+                local_p2,
+                n1: rb1.predicted_position * local_n1,
+                radius,
+                im2: rb2.mass_properties.inv_mass,
+                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                num_contacts: manifold_points.len() as u8,
+                erp: params.erp,
+                max_linear_correction: params.max_linear_correction,
+            };
+
+            if push {
+                if manifold.kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints.push(AnyPositionConstraint::NongroupedPointPointGround(
+                        constraint,
+                    ));
+                } else {
+                    out_constraints.push(AnyPositionConstraint::NongroupedPlanePointGround(
+                        constraint,
+                    ));
+                }
+            } else {
+                if manifold.kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints[manifold.constraint_index + l] =
+                        AnyPositionConstraint::NongroupedPointPointGround(constraint);
+                } else {
+                    out_constraints[manifold.constraint_index + l] =
+                        AnyPositionConstraint::NongroupedPlanePointGround(constraint);
+                }
+            }
+        }
+    }
+    pub fn solve_point_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // 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;
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let p1 = self.p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+
+            let sqdist = dpos.norm_squared();
+
+            // NOTE: only works for the point-point case.
+            if sqdist < target_dist * target_dist {
+                let dist = sqdist.sqrt();
+                let n = dpos / dist;
+                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross2 = -dp2.gcross(n);
+                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(n * (-impulse * self.im2));
+                let rot2 = Rotation::new(ii_gcross2 * impulse);
+                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
+            }
+        }
+
+        positions[self.rb2] = pos2;
+    }
+
+    pub fn solve_plane_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // 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;
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let n1 = self.n1;
+            let p1 = self.p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+            let dist = dpos.dot(&n1);
+
+            if dist < target_dist {
+                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross2 = -dp2.gcross(n1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                let tra2 = Translation::from(n1 * (-impulse * self.im2));
+                let rot2 = Rotation::new(ii_gcross2 * impulse);
+                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
+            }
+        }
+
+        positions[self.rb2] = pos2;
+    }
+}

src/dynamics/solver/position_ground_constraint_wide.rs

@@ -0,0 +1,199 @@
+use super::AnyPositionConstraint;
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, KinematicsCategory};
+use crate::math::{
+    AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
+    SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WDot};
+
+use num::Zero;
+use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue};
+
+pub(crate) struct WPositionGroundConstraint {
+    pub rb2: [usize; SIMD_WIDTH],
+    // NOTE: the points are relative to the center of masses.
+    pub p1: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
+    pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
+    pub n1: Vector<SimdFloat>,
+    pub radius: SimdFloat,
+    pub im2: SimdFloat,
+    pub ii2: AngularInertia<SimdFloat>,
+    pub erp: SimdFloat,
+    pub max_linear_correction: SimdFloat,
+    pub num_contacts: u8,
+}
+
+impl WPositionGroundConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifolds: [&ContactManifold; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyPositionConstraint>,
+        push: bool,
+    ) {
+        let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
+        let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
+        let mut flipped = [false; SIMD_WIDTH];
+
+        for ii in 0..SIMD_WIDTH {
+            if !rbs2[ii].is_dynamic() {
+                flipped[ii] = true;
+                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+            }
+        }
+
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let sqrt_ii2: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+
+        let local_n1 = Vector::from(
+            array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
+        );
+        let local_n2 = Vector::from(
+            array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH],
+        );
+
+        let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
+        let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
+
+        let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
+
+        let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
+
+        let n1 = position1 * local_n1;
+
+        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
+            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+            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,
+                radius,
+                im2,
+                ii2: sqrt_ii2.squared(),
+                erp: SimdFloat::splat(params.erp),
+                max_linear_correction: SimdFloat::splat(params.max_linear_correction),
+                num_contacts: num_points as u8,
+            };
+
+            for i in 0..num_points {
+                let local_p1 = Point::from(
+                    array![|ii| if flipped[ii] { manifold_points[ii][i].local_p2 } else { manifold_points[ii][i].local_p1 }; SIMD_WIDTH],
+                );
+                let local_p2 = Point::from(
+                    array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH],
+                );
+
+                constraint.p1[i] = position1 * local_p1 - n1 * radius1;
+                constraint.local_p2[i] = local_p2 - local_n2 * radius2;
+            }
+
+            if push {
+                if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints
+                        .push(AnyPositionConstraint::GroupedPointPointGround(constraint));
+                } else {
+                    out_constraints
+                        .push(AnyPositionConstraint::GroupedPlanePointGround(constraint));
+                }
+            } else {
+                if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                        AnyPositionConstraint::GroupedPointPointGround(constraint);
+                } else {
+                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                        AnyPositionConstraint::GroupedPlanePointGround(constraint);
+                }
+            }
+        }
+    }
+    pub fn solve_point_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let allowed_err = SimdFloat::splat(params.allowed_linear_error);
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let p1 = self.p1[k];
+            let p2 = pos2 * self.local_p2[k];
+
+            let dpos = p2 - p1;
+            let sqdist = dpos.norm_squared();
+
+            if sqdist.simd_lt(target_dist * target_dist).any() {
+                let dist = sqdist.simd_sqrt();
+                let n = dpos / dist;
+                let err = ((dist - target_dist) * self.erp)
+                    .simd_clamp(-self.max_linear_correction, SimdFloat::zero());
+                let dp2 = p2.coords - pos2.translation.vector;
+                let gcross2 = -dp2.gcross(n);
+                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(n * (-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);
+        }
+    }
+
+    pub fn solve_plane_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let allowed_err = SimdFloat::splat(params.allowed_linear_error);
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            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, SimdFloat::zero());
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross2 = -dp2.gcross(n1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation;
+                pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
+            }
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.rb2[ii]] = pos2.extract(ii);
+        }
+    }
+}

src/dynamics/solver/position_solver.rs

@@ -0,0 +1,451 @@
+use super::{
+    AnyJointPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint,
+};
+#[cfg(feature = "simd-is-enabled")]
+use super::{WPositionConstraint, WPositionGroundConstraint};
+use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts};
+use crate::dynamics::{
+    solver::AnyPositionConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
+};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::Isometry;
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::SIMD_WIDTH;
+
+pub(crate) struct PositionSolverJointPart {
+    pub nonground_joints: Vec<JointIndex>,
+    pub ground_joints: Vec<JointIndex>,
+    pub nonground_joint_groups: InteractionGroups,
+    pub ground_joint_groups: InteractionGroups,
+    pub constraints: Vec<AnyJointPositionConstraint>,
+}
+
+impl PositionSolverJointPart {
+    pub fn new() -> Self {
+        Self {
+            nonground_joints: Vec::new(),
+            ground_joints: Vec::new(),
+            nonground_joint_groups: InteractionGroups::new(),
+            ground_joint_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+        }
+    }
+}
+
+pub(crate) struct PositionSolverPart {
+    pub point_point_manifolds: Vec<ContactManifoldIndex>,
+    pub plane_point_manifolds: Vec<ContactManifoldIndex>,
+    pub point_point_ground_manifolds: Vec<ContactManifoldIndex>,
+    pub plane_point_ground_manifolds: Vec<ContactManifoldIndex>,
+    pub point_point_groups: InteractionGroups,
+    pub plane_point_groups: InteractionGroups,
+    pub point_point_ground_groups: InteractionGroups,
+    pub plane_point_ground_groups: InteractionGroups,
+    pub constraints: Vec<AnyPositionConstraint>,
+}
+
+impl PositionSolverPart {
+    pub fn new() -> Self {
+        Self {
+            point_point_manifolds: Vec::new(),
+            plane_point_manifolds: Vec::new(),
+            point_point_ground_manifolds: Vec::new(),
+            plane_point_ground_manifolds: Vec::new(),
+            point_point_groups: InteractionGroups::new(),
+            plane_point_groups: InteractionGroups::new(),
+            point_point_ground_groups: InteractionGroups::new(),
+            plane_point_ground_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+        }
+    }
+}
+
+pub(crate) struct PositionSolver {
+    positions: Vec<Isometry<f32>>,
+    part: PositionSolverPart,
+    joint_part: PositionSolverJointPart,
+}
+
+impl PositionSolver {
+    pub fn new() -> Self {
+        Self {
+            positions: Vec::new(),
+            part: PositionSolverPart::new(),
+            joint_part: PositionSolverJointPart::new(),
+        }
+    }
+
+    pub fn init_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+        joints: &[JointGraphEdge],
+        joint_constraint_indices: &[JointIndex],
+    ) {
+        self.part
+            .init_constraints(island_id, params, bodies, manifolds, manifold_indices);
+        self.joint_part.init_constraints(
+            island_id,
+            params,
+            bodies,
+            joints,
+            joint_constraint_indices,
+        );
+    }
+
+    pub fn solve_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+    ) {
+        self.positions.clear();
+        self.positions.extend(
+            bodies
+                .iter_active_island(island_id)
+                .map(|(_, b)| b.position),
+        );
+
+        for _ in 0..params.max_position_iterations {
+            for constraint in &self.joint_part.constraints {
+                constraint.solve(params, &mut self.positions)
+            }
+
+            for constraint in &self.part.constraints {
+                constraint.solve(params, &mut self.positions)
+            }
+        }
+
+        bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
+            rb.set_position(self.positions[rb.active_set_offset])
+        });
+    }
+}
+
+impl PositionSolverPart {
+    pub fn init_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+    ) {
+        self.point_point_ground_manifolds.clear();
+        self.plane_point_ground_manifolds.clear();
+        self.point_point_manifolds.clear();
+        self.plane_point_manifolds.clear();
+        categorize_position_contacts(
+            bodies,
+            manifolds_all,
+            manifold_indices,
+            &mut self.point_point_ground_manifolds,
+            &mut self.plane_point_ground_manifolds,
+            &mut self.point_point_manifolds,
+            &mut self.plane_point_manifolds,
+        );
+
+        self.point_point_groups.clear_groups();
+        self.point_point_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds_all,
+            &self.point_point_manifolds,
+        );
+        self.plane_point_groups.clear_groups();
+        self.plane_point_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds_all,
+            &self.plane_point_manifolds,
+        );
+
+        self.point_point_ground_groups.clear_groups();
+        self.point_point_ground_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds_all,
+            &self.point_point_ground_manifolds,
+        );
+        self.plane_point_ground_groups.clear_groups();
+        self.plane_point_ground_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds_all,
+            &self.plane_point_ground_manifolds,
+        );
+
+        self.constraints.clear();
+
+        /*
+         * Init non-ground contact constraints.
+         */
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            compute_grouped_constraints(
+                params,
+                bodies,
+                manifolds_all,
+                &self.point_point_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+            compute_grouped_constraints(
+                params,
+                bodies,
+                manifolds_all,
+                &self.plane_point_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+        }
+        compute_nongrouped_constraints(
+            params,
+            bodies,
+            manifolds_all,
+            &self.point_point_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+        compute_nongrouped_constraints(
+            params,
+            bodies,
+            manifolds_all,
+            &self.plane_point_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+
+        /*
+         * Init ground contact constraints.
+         */
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            compute_grouped_ground_constraints(
+                params,
+                bodies,
+                manifolds_all,
+                &self.point_point_ground_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+            compute_grouped_ground_constraints(
+                params,
+                bodies,
+                manifolds_all,
+                &self.plane_point_ground_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+        }
+        compute_nongrouped_ground_constraints(
+            params,
+            bodies,
+            manifolds_all,
+            &self.point_point_ground_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+        compute_nongrouped_ground_constraints(
+            params,
+            bodies,
+            manifolds_all,
+            &self.plane_point_ground_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+    }
+}
+
+impl PositionSolverJointPart {
+    pub fn init_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints: &[JointGraphEdge],
+        joint_constraint_indices: &[JointIndex],
+    ) {
+        self.ground_joints.clear();
+        self.nonground_joints.clear();
+        categorize_joints(
+            bodies,
+            joints,
+            joint_constraint_indices,
+            &mut self.ground_joints,
+            &mut self.nonground_joints,
+        );
+
+        self.nonground_joint_groups.clear_groups();
+        self.nonground_joint_groups
+            .group_joints(island_id, bodies, joints, &self.nonground_joints);
+
+        self.ground_joint_groups.clear_groups();
+        self.ground_joint_groups
+            .group_joints(island_id, bodies, joints, &self.ground_joints);
+
+        /*
+         * Init ground joint constraints.
+         */
+        self.constraints.clear();
+        compute_nongrouped_joint_ground_constraints(
+            params,
+            bodies,
+            joints,
+            &self.ground_joint_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            compute_grouped_joint_ground_constraints(
+                params,
+                bodies,
+                joints,
+                &self.ground_joint_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+        }
+
+        /*
+         * Init non-ground joint constraints.
+         */
+        compute_nongrouped_joint_constraints(
+            params,
+            bodies,
+            joints,
+            &self.nonground_joint_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            compute_grouped_joint_constraints(
+                params,
+                bodies,
+                joints,
+                &self.nonground_joint_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+        }
+    }
+}
+
+fn compute_nongrouped_constraints(
+    params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    manifolds_all: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    output: &mut Vec<AnyPositionConstraint>,
+) {
+    for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
+        PositionConstraint::generate(params, manifold, bodies, output, true)
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+fn compute_grouped_constraints(
+    params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    manifolds_all: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    output: &mut Vec<AnyPositionConstraint>,
+) {
+    for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
+        let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+        WPositionConstraint::generate(params, manifolds, bodies, output, true)
+    }
+}
+
+fn compute_nongrouped_ground_constraints(
+    params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    manifolds_all: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    output: &mut Vec<AnyPositionConstraint>,
+) {
+    for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
+        PositionGroundConstraint::generate(params, manifold, bodies, output, true)
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+fn compute_grouped_ground_constraints(
+    params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    manifolds_all: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    output: &mut Vec<AnyPositionConstraint>,
+) {
+    for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
+        let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+        WPositionGroundConstraint::generate(params, manifolds, bodies, output, true);
+    }
+}
+
+fn compute_nongrouped_joint_ground_constraints(
+    _params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    joints_all: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    output: &mut Vec<AnyJointPositionConstraint>,
+) {
+    for joint_i in joint_indices {
+        let joint = &joints_all[*joint_i].weight;
+        let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
+        output.push(pos_constraint);
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+fn compute_grouped_joint_ground_constraints(
+    _params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    joints_all: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    output: &mut Vec<AnyJointPositionConstraint>,
+) {
+    for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
+        let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
+        if let Some(pos_constraint) =
+            AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies)
+        {
+            output.push(pos_constraint);
+        } else {
+            for joint in joints.iter() {
+                output.push(AnyJointPositionConstraint::from_joint_ground(
+                    *joint, bodies,
+                ))
+            }
+        }
+    }
+}
+
+fn compute_nongrouped_joint_constraints(
+    _params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    joints_all: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    output: &mut Vec<AnyJointPositionConstraint>,
+) {
+    for joint_i in joint_indices {
+        let joint = &joints_all[*joint_i];
+        let pos_constraint = AnyJointPositionConstraint::from_joint(&joint.weight, bodies);
+        output.push(pos_constraint);
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+fn compute_grouped_joint_constraints(
+    _params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    joints_all: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    output: &mut Vec<AnyJointPositionConstraint>,
+) {
+    for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
+        let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
+        if let Some(pos_constraint) = AnyJointPositionConstraint::from_wide_joint(joints, bodies) {
+            output.push(pos_constraint);
+        } else {
+            for joint in joints.iter() {
+                output.push(AnyJointPositionConstraint::from_joint(*joint, bodies))
+            }
+        }
+    }
+}

src/dynamics/solver/velocity_constraint.rs

@@ -0,0 +1,401 @@
+use super::DeltaVel;
+use crate::dynamics::solver::VelocityGroundConstraint;
+#[cfg(feature = "simd-is-enabled")]
+use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS};
+use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
+use simba::simd::SimdPartialOrd;
+
+//#[repr(align(64))]
+#[derive(Copy, Clone, Debug)]
+pub(crate) enum AnyVelocityConstraint {
+    NongroupedGround(VelocityGroundConstraint),
+    Nongrouped(VelocityConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedGround(WVelocityGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    Grouped(WVelocityConstraint),
+    #[allow(dead_code)] // The Empty variant is only used with parallel code.
+    Empty,
+}
+
+impl AnyVelocityConstraint {
+    #[cfg(target_arch = "wasm32")]
+    pub fn as_nongrouped_mut(&mut self) -> Option<&mut VelocityConstraint> {
+        if let AnyVelocityConstraint::Nongrouped(c) = self {
+            Some(c)
+        } else {
+            None
+        }
+    }
+
+    #[cfg(target_arch = "wasm32")]
+    pub fn as_nongrouped_ground_mut(&mut self) -> Option<&mut VelocityGroundConstraint> {
+        if let AnyVelocityConstraint::NongroupedGround(c) = self {
+            Some(c)
+        } else {
+            None
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        match self {
+            AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas),
+            AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::GroupedGround(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::Grouped(c) => c.warmstart(mj_lambdas),
+            AnyVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        match self {
+            AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas),
+            AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::GroupedGround(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas),
+            AnyVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+
+    pub fn writeback_impulses(&self, manifold_all: &mut [&mut ContactManifold]) {
+        match self {
+            AnyVelocityConstraint::NongroupedGround(c) => c.writeback_impulses(manifold_all),
+            AnyVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifold_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all),
+            AnyVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityConstraintElementPart {
+    pub gcross1: AngVector<f32>,
+    pub gcross2: AngVector<f32>,
+    pub rhs: f32,
+    pub impulse: f32,
+    pub r: f32,
+}
+
+#[cfg(not(target_arch = "wasm32"))]
+impl VelocityConstraintElementPart {
+    fn zero() -> Self {
+        Self {
+            gcross1: na::zero(),
+            gcross2: na::zero(),
+            rhs: 0.0,
+            impulse: 0.0,
+            r: 0.0,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityConstraintElement {
+    pub normal_part: VelocityConstraintElementPart,
+    pub tangent_part: [VelocityConstraintElementPart; DIM - 1],
+}
+
+#[cfg(not(target_arch = "wasm32"))]
+impl VelocityConstraintElement {
+    pub fn zero() -> Self {
+        Self {
+            normal_part: VelocityConstraintElementPart::zero(),
+            tangent_part: [VelocityConstraintElementPart::zero(); DIM - 1],
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityConstraint {
+    pub dir1: Vector<f32>, // Non-penetration force direction for the first body.
+    pub im1: f32,
+    pub im2: f32,
+    pub limit: f32,
+    pub mj_lambda1: usize,
+    pub mj_lambda2: usize,
+    pub manifold_id: ContactManifoldIndex,
+    pub manifold_contact_id: usize,
+    pub num_contacts: u8,
+    pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS],
+}
+
+impl VelocityConstraint {
+    #[cfg(feature = "parallel")]
+    pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
+        let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
+        manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
+    }
+
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold_id: ContactManifoldIndex,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
+        push: bool,
+    ) {
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+        let mj_lambda1 = rb1.active_set_offset;
+        let mj_lambda2 = rb2.active_set_offset;
+        let force_dir1 = rb1.position * (-manifold.local_n1);
+        let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
+
+        for (l, manifold_points) in manifold
+            .active_contacts()
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            #[cfg(not(target_arch = "wasm32"))]
+            let mut constraint = VelocityConstraint {
+                dir1: force_dir1,
+                elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+                im1: rb1.mass_properties.inv_mass,
+                im2: rb2.mass_properties.inv_mass,
+                limit: manifold.friction,
+                mj_lambda1,
+                mj_lambda2,
+                manifold_id,
+                manifold_contact_id: l * MAX_MANIFOLD_POINTS,
+                num_contacts: manifold_points.len() as u8,
+            };
+
+            // TODO: this is a WIP optimization for WASM platforms.
+            // For some reasons, the compiler does not inline the `Vec::push` method
+            // in this method. This generates two memset and one memcpy which are both very
+            // expansive.
+            // This would likely be solved by some kind of "placement-push" (like emplace in C++).
+            // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
+            // avoid spurious copying.
+            // Is this optimization beneficial when targeting non-WASM platforms?
+            //
+            // NOTE: 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 {
+                let new_len = out_constraints.len() + 1;
+                unsafe {
+                    out_constraints.resize_with(new_len, || {
+                        AnyVelocityConstraint::Nongrouped(
+                            std::mem::MaybeUninit::uninit().assume_init(),
+                        )
+                    });
+                }
+                out_constraints
+                    .last_mut()
+                    .unwrap()
+                    .as_nongrouped_mut()
+                    .unwrap()
+            } else {
+                unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
+            };
+
+            #[cfg(target_arch = "wasm32")]
+            {
+                constraint.dir1 = force_dir1;
+                constraint.im1 = rb1.mass_properties.inv_mass;
+                constraint.im2 = rb2.mass_properties.inv_mass;
+                constraint.limit = manifold.friction;
+                constraint.mj_lambda1 = mj_lambda1;
+                constraint.mj_lambda2 = mj_lambda2;
+                constraint.manifold_id = manifold_id;
+                constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
+                constraint.num_contacts = manifold_points.len() as u8;
+            }
+
+            for k in 0..manifold_points.len() {
+                let manifold_point = &manifold_points[k];
+                let dp1 = (rb1.position * manifold_point.local_p1).coords
+                    - rb1.position.translation.vector;
+                let dp2 = (rb2.position * manifold_point.local_p2).coords
+                    - rb2.position.translation.vector;
+
+                let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
+                let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
+
+                // Normal part.
+                {
+                    let gcross1 = rb1
+                        .world_inv_inertia_sqrt
+                        .transform_vector(dp1.gcross(force_dir1));
+                    let gcross2 = rb2
+                        .world_inv_inertia_sqrt
+                        .transform_vector(dp2.gcross(-force_dir1));
+
+                    let r = 1.0
+                        / (rb1.mass_properties.inv_mass
+                            + rb2.mass_properties.inv_mass
+                            + gcross1.gdot(gcross1)
+                            + gcross2.gdot(gcross2));
+
+                    let rhs = (vel1 - vel2).dot(&force_dir1)
+                        + manifold_point.dist.max(0.0) * params.inv_dt();
+
+                    let impulse = manifold_points[k].impulse * warmstart_coeff;
+
+                    constraint.elements[k].normal_part = VelocityConstraintElementPart {
+                        gcross1,
+                        gcross2,
+                        rhs,
+                        impulse,
+                        r,
+                    };
+                }
+
+                // Tangent parts.
+                {
+                    let tangents1 = force_dir1.orthonormal_basis();
+
+                    for j in 0..DIM - 1 {
+                        let gcross1 = rb1
+                            .world_inv_inertia_sqrt
+                            .transform_vector(dp1.gcross(tangents1[j]));
+                        let gcross2 = rb2
+                            .world_inv_inertia_sqrt
+                            .transform_vector(dp2.gcross(-tangents1[j]));
+                        let r = 1.0
+                            / (rb1.mass_properties.inv_mass
+                                + rb2.mass_properties.inv_mass
+                                + gcross1.gdot(gcross1)
+                                + gcross2.gdot(gcross2));
+                        let rhs = (vel1 - vel2).dot(&tangents1[j]);
+                        #[cfg(feature = "dim2")]
+                        let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
+                        #[cfg(feature = "dim3")]
+                        let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
+
+                        constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart {
+                            gcross1,
+                            gcross2,
+                            rhs,
+                            impulse,
+                            r,
+                        };
+                    }
+                }
+            }
+
+            #[cfg(not(target_arch = "wasm32"))]
+            if push {
+                out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
+            } else {
+                out_constraints[manifold.constraint_index + l] =
+                    AnyVelocityConstraint::Nongrouped(constraint);
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel::zero();
+        let mut mj_lambda2 = DeltaVel::zero();
+
+        for i in 0..self.num_contacts as usize {
+            let elt = &self.elements[i].normal_part;
+            mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse);
+            mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
+            mj_lambda2.angular += elt.gcross2 * elt.impulse;
+
+            // FIXME: move this out of the for loop?
+            let tangents1 = self.dir1.orthonormal_basis();
+
+            for j in 0..DIM - 1 {
+                let elt = &self.elements[i].tangent_part[j];
+                mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse);
+                mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
+                mj_lambda2.angular += elt.gcross2 * elt.impulse;
+            }
+        }
+
+        mj_lambdas[self.mj_lambda1 as usize].linear += mj_lambda1.linear;
+        mj_lambdas[self.mj_lambda1 as usize].angular += mj_lambda1.angular;
+        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<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        // Solve friction.
+        for i in 0..self.num_contacts as usize {
+            let tangents1 = self.dir1.orthonormal_basis();
+
+            for j in 0..DIM - 1 {
+                let normal_elt = &self.elements[i].normal_part;
+                let elt = &mut self.elements[i].tangent_part[j];
+                let dimpulse = tangents1[j].dot(&mj_lambda1.linear)
+                    + elt.gcross1.gdot(mj_lambda1.angular)
+                    - tangents1[j].dot(&mj_lambda2.linear)
+                    + elt.gcross2.gdot(mj_lambda2.angular)
+                    + elt.rhs;
+                let limit = self.limit * normal_elt.impulse;
+                let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
+                let dlambda = new_impulse - elt.impulse;
+                elt.impulse = new_impulse;
+
+                mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda);
+                mj_lambda1.angular += elt.gcross1 * dlambda;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
+                mj_lambda2.angular += elt.gcross2 * dlambda;
+            }
+        }
+
+        // Solve penetration.
+        for i in 0..self.num_contacts as usize {
+            let elt = &mut self.elements[i].normal_part;
+            let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular)
+                - self.dir1.dot(&mj_lambda2.linear)
+                + elt.gcross2.gdot(mj_lambda2.angular)
+                + elt.rhs;
+            let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0);
+            let dlambda = new_impulse - elt.impulse;
+            elt.impulse = new_impulse;
+
+            mj_lambda1.linear += self.dir1 * (self.im1 * dlambda);
+            mj_lambda1.angular += elt.gcross1 * dlambda;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
+            mj_lambda2.angular += elt.gcross2 * dlambda;
+        }
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        let manifold = &mut manifolds_all[self.manifold_id];
+        let k_base = self.manifold_contact_id;
+
+        for k in 0..self.num_contacts as usize {
+            let active_contacts = manifold.active_contacts_mut();
+            active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
+            #[cfg(feature = "dim2")]
+            {
+                active_contacts[k_base + k].tangent_impulse =
+                    self.elements[k].tangent_part[0].impulse;
+            }
+            #[cfg(feature = "dim3")]
+            {
+                active_contacts[k_base + k].tangent_impulse = [
+                    self.elements[k].tangent_part[0].impulse,
+                    self.elements[k].tangent_part[1].impulse,
+                ];
+            }
+        }
+    }
+}

src/dynamics/solver/velocity_constraint_wide.rs

@@ -0,0 +1,344 @@
+use super::{AnyVelocityConstraint, DeltaVel};
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS,
+    SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
+use num::Zero;
+use simba::simd::{SimdPartialOrd, SimdValue};
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityConstraintElementPart {
+    pub gcross1: AngVector<SimdFloat>,
+    pub gcross2: AngVector<SimdFloat>,
+    pub rhs: SimdFloat,
+    pub impulse: SimdFloat,
+    pub r: SimdFloat,
+}
+
+impl WVelocityConstraintElementPart {
+    pub fn zero() -> Self {
+        Self {
+            gcross1: AngVector::zero(),
+            gcross2: AngVector::zero(),
+            rhs: SimdFloat::zero(),
+            impulse: SimdFloat::zero(),
+            r: SimdFloat::zero(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityConstraintElement {
+    pub normal_part: WVelocityConstraintElementPart,
+    pub tangent_parts: [WVelocityConstraintElementPart; DIM - 1],
+}
+
+impl WVelocityConstraintElement {
+    pub fn zero() -> Self {
+        Self {
+            normal_part: WVelocityConstraintElementPart::zero(),
+            tangent_parts: [WVelocityConstraintElementPart::zero(); DIM - 1],
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityConstraint {
+    pub dir1: Vector<SimdFloat>, // Non-penetration force direction for the first body.
+    pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS],
+    pub num_contacts: u8,
+    pub im1: SimdFloat,
+    pub im2: SimdFloat,
+    pub limit: SimdFloat,
+    pub mj_lambda1: [usize; SIMD_WIDTH],
+    pub mj_lambda2: [usize; SIMD_WIDTH],
+    pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+    pub manifold_contact_id: usize,
+}
+
+impl WVelocityConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+        manifolds: [&ContactManifold; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
+        push: bool,
+    ) {
+        let inv_dt = SimdFloat::splat(params.inv_dt());
+        let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
+        let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
+
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+
+        let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
+
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
+        let warmstart_multiplier =
+            SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
+        let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
+
+        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
+            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+            let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
+
+            let mut constraint = WVelocityConstraint {
+                dir1: force_dir1,
+                elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+                im1,
+                im2,
+                limit: friction,
+                mj_lambda1,
+                mj_lambda2,
+                manifold_id,
+                manifold_contact_id: l,
+                num_contacts: num_points as u8,
+            };
+
+            for k in 0..num_points {
+                // FIXME: can we avoid the multiplications by position1/position2 here?
+                // By working as much as possible in local-space.
+                let p1 = position1
+                    * Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]);
+                let p2 = position2
+                    * Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]);
+
+                let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
+
+                let impulse =
+                    SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
+
+                let dp1 = p1.coords - position1.translation.vector;
+                let dp2 = p2.coords - position2.translation.vector;
+
+                let vel1 = linvel1 + angvel1.gcross(dp1);
+                let vel2 = linvel2 + angvel2.gcross(dp2);
+
+                // Normal part.
+                {
+                    let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
+                    let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
+
+                    let r = SimdFloat::splat(1.0)
+                        / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
+                    let rhs =
+                        (vel1 - vel2).dot(&force_dir1) + dist.simd_max(SimdFloat::zero()) * inv_dt;
+
+                    constraint.elements[k].normal_part = WVelocityConstraintElementPart {
+                        gcross1,
+                        gcross2,
+                        rhs,
+                        impulse: impulse * warmstart_coeff,
+                        r,
+                    };
+                }
+
+                // tangent parts.
+                let tangents1 = force_dir1.orthonormal_basis();
+
+                for j in 0..DIM - 1 {
+                    #[cfg(feature = "dim2")]
+                    let impulse = SimdFloat::from(
+                        array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH],
+                    );
+                    #[cfg(feature = "dim3")]
+                    let impulse = SimdFloat::from(
+                        array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH],
+                    );
+
+                    let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
+                    let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
+                    let r = SimdFloat::splat(1.0)
+                        / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
+                    let rhs = (vel1 - vel2).dot(&tangents1[j]);
+
+                    constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart {
+                        gcross1,
+                        gcross2,
+                        rhs,
+                        impulse: impulse * warmstart_coeff,
+                        r,
+                    };
+                }
+            }
+
+            if push {
+                out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
+            } else {
+                out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    AnyVelocityConstraint::Grouped(constraint);
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        for i in 0..self.num_contacts as usize {
+            let elt = &self.elements[i].normal_part;
+            mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse);
+            mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
+            mj_lambda2.angular += elt.gcross2 * elt.impulse;
+
+            // FIXME: move this out of the for loop?
+            let tangents1 = self.dir1.orthonormal_basis();
+
+            for j in 0..DIM - 1 {
+                let elt = &self.elements[i].tangent_parts[j];
+                mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse);
+                mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
+                mj_lambda2.angular += elt.gcross2 * elt.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<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        // Solve friction first.
+        for i in 0..self.num_contacts as usize {
+            // FIXME: move this out of the for loop?
+            let tangents1 = self.dir1.orthonormal_basis();
+            let normal_elt = &self.elements[i].normal_part;
+
+            for j in 0..DIM - 1 {
+                let elt = &mut self.elements[i].tangent_parts[j];
+                let dimpulse = tangents1[j].dot(&mj_lambda1.linear)
+                    + elt.gcross1.gdot(mj_lambda1.angular)
+                    - tangents1[j].dot(&mj_lambda2.linear)
+                    + elt.gcross2.gdot(mj_lambda2.angular)
+                    + elt.rhs;
+                let limit = self.limit * normal_elt.impulse;
+                let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
+                let dlambda = new_impulse - elt.impulse;
+                elt.impulse = new_impulse;
+
+                mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda);
+                mj_lambda1.angular += elt.gcross1 * dlambda;
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
+                mj_lambda2.angular += elt.gcross2 * dlambda;
+            }
+        }
+
+        // Solve non-penetration after friction.
+        for i in 0..self.num_contacts as usize {
+            let elt = &mut self.elements[i].normal_part;
+            let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular)
+                - self.dir1.dot(&mj_lambda2.linear)
+                + elt.gcross2.gdot(mj_lambda2.angular)
+                + elt.rhs;
+            let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero());
+            let dlambda = new_impulse - elt.impulse;
+            elt.impulse = new_impulse;
+
+            mj_lambda1.linear += self.dir1 * (self.im1 * dlambda);
+            mj_lambda1.angular += elt.gcross1 * dlambda;
+            mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
+            mj_lambda2.angular += elt.gcross2 * dlambda;
+        }
+
+        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, 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 tangent_impulses: [_; SIMD_WIDTH] =
+                self.elements[k].tangent_parts[0].impulse.into();
+            #[cfg(feature = "dim3")]
+            let bitangent_impulses: [_; SIMD_WIDTH] =
+                self.elements[k].tangent_parts[1].impulse.into();
+
+            for ii in 0..SIMD_WIDTH {
+                let manifold = &mut manifolds_all[self.manifold_id[ii]];
+                let k_base = self.manifold_contact_id;
+                let active_contacts = manifold.active_contacts_mut();
+                active_contacts[k_base + k].impulse = impulses[ii];
+
+                #[cfg(feature = "dim2")]
+                {
+                    active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii];
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    active_contacts[k_base + k].tangent_impulse =
+                        [tangent_impulses[ii], bitangent_impulses[ii]];
+                }
+            }
+        }
+    }
+}

src/dynamics/solver/velocity_ground_constraint.rs

@@ -0,0 +1,300 @@
+use super::{AnyVelocityConstraint, DeltaVel};
+use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS};
+use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
+
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use simba::simd::SimdPartialOrd;
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityGroundConstraintElementPart {
+    pub gcross2: AngVector<f32>,
+    pub rhs: f32,
+    pub impulse: f32,
+    pub r: f32,
+}
+
+#[cfg(not(target_arch = "wasm32"))]
+impl VelocityGroundConstraintElementPart {
+    fn zero() -> Self {
+        Self {
+            gcross2: na::zero(),
+            rhs: 0.0,
+            impulse: 0.0,
+            r: 0.0,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityGroundConstraintElement {
+    pub normal_part: VelocityGroundConstraintElementPart,
+    pub tangent_part: [VelocityGroundConstraintElementPart; DIM - 1],
+}
+
+#[cfg(not(target_arch = "wasm32"))]
+impl VelocityGroundConstraintElement {
+    pub fn zero() -> Self {
+        Self {
+            normal_part: VelocityGroundConstraintElementPart::zero(),
+            tangent_part: [VelocityGroundConstraintElementPart::zero(); DIM - 1],
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityGroundConstraint {
+    pub dir1: Vector<f32>, // Non-penetration force direction for the first body.
+    pub im2: f32,
+    pub limit: f32,
+    pub mj_lambda2: usize,
+    pub manifold_id: ContactManifoldIndex,
+    pub manifold_contact_id: usize,
+    pub num_contacts: u8,
+    pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
+}
+
+impl VelocityGroundConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold_id: ContactManifoldIndex,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
+        push: bool,
+    ) {
+        let mut rb1 = &bodies[manifold.body_pair.body1];
+        let mut rb2 = &bodies[manifold.body_pair.body2];
+        let flipped = !rb2.is_dynamic();
+
+        if flipped {
+            std::mem::swap(&mut rb1, &mut rb2);
+        }
+
+        let mj_lambda2 = rb2.active_set_offset;
+        let force_dir1 = if flipped {
+            // NOTE: we already swapped rb1 and rb2
+            // so we multiply by rb1.position.
+            rb1.position * (-manifold.local_n2)
+        } else {
+            rb1.position * (-manifold.local_n1)
+        };
+
+        let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
+
+        for (l, manifold_points) in manifold
+            .active_contacts()
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            #[cfg(not(target_arch = "wasm32"))]
+            let mut constraint = VelocityGroundConstraint {
+                dir1: force_dir1,
+                elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+                im2: rb2.mass_properties.inv_mass,
+                limit: manifold.friction,
+                mj_lambda2,
+                manifold_id,
+                manifold_contact_id: l * MAX_MANIFOLD_POINTS,
+                num_contacts: manifold_points.len() as u8,
+            };
+
+            // TODO: this is a WIP optimization for WASM platforms.
+            // For some reasons, the compiler does not inline the `Vec::push` method
+            // in this method. This generates two memset and one memcpy which are both very
+            // expansive.
+            // This would likely be solved by some kind of "placement-push" (like emplace in C++).
+            // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
+            // avoid spurious copying.
+            // Is this optimization beneficial when targeting non-WASM platforms?
+            //
+            // NOTE: 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 {
+                let new_len = out_constraints.len() + 1;
+                unsafe {
+                    out_constraints.resize_with(new_len, || {
+                        AnyVelocityConstraint::NongroupedGround(
+                            std::mem::MaybeUninit::uninit().assume_init(),
+                        )
+                    });
+                }
+                out_constraints
+                    .last_mut()
+                    .unwrap()
+                    .as_nongrouped_ground_mut()
+                    .unwrap()
+            } else {
+                unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
+            };
+
+            #[cfg(target_arch = "wasm32")]
+            {
+                constraint.dir1 = force_dir1;
+                constraint.im2 = rb2.mass_properties.inv_mass;
+                constraint.limit = manifold.friction;
+                constraint.mj_lambda2 = mj_lambda2;
+                constraint.manifold_id = manifold_id;
+                constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
+                constraint.num_contacts = manifold_points.len() as u8;
+            }
+
+            for k in 0..manifold_points.len() {
+                let manifold_point = &manifold_points[k];
+                let (p1, p2) = if flipped {
+                    // NOTE: we already swapped rb1 and rb2
+                    // so we multiply by rb2.position.
+                    (
+                        rb1.position * manifold_point.local_p2,
+                        rb2.position * manifold_point.local_p1,
+                    )
+                } else {
+                    (
+                        rb1.position * manifold_point.local_p1,
+                        rb2.position * manifold_point.local_p2,
+                    )
+                };
+                let dp2 = p2.coords - rb2.position.translation.vector;
+                let dp1 = p1.coords - rb1.position.translation.vector;
+                let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
+                let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
+
+                // Normal part.
+                {
+                    let gcross2 = rb2
+                        .world_inv_inertia_sqrt
+                        .transform_vector(dp2.gcross(-force_dir1));
+
+                    let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
+                    let rhs = -vel2.dot(&force_dir1)
+                        + vel1.dot(&force_dir1)
+                        + manifold_point.dist.max(0.0) * params.inv_dt();
+                    let impulse = manifold_points[k].impulse * warmstart_coeff;
+
+                    constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
+                        gcross2,
+                        rhs,
+                        impulse,
+                        r,
+                    };
+                }
+
+                // Tangent parts.
+                {
+                    let tangents1 = force_dir1.orthonormal_basis();
+
+                    for j in 0..DIM - 1 {
+                        let gcross2 = rb2
+                            .world_inv_inertia_sqrt
+                            .transform_vector(dp2.gcross(-tangents1[j]));
+                        let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
+                        let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
+                        #[cfg(feature = "dim2")]
+                        let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
+                        #[cfg(feature = "dim3")]
+                        let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
+
+                        constraint.elements[k].tangent_part[j] =
+                            VelocityGroundConstraintElementPart {
+                                gcross2,
+                                rhs,
+                                impulse,
+                                r,
+                            };
+                    }
+                }
+            }
+
+            #[cfg(not(target_arch = "wasm32"))]
+            if push {
+                out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
+            } else {
+                out_constraints[manifold.constraint_index + l] =
+                    AnyVelocityConstraint::NongroupedGround(constraint);
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel::zero();
+        let tangents1 = self.dir1.orthonormal_basis();
+
+        for i in 0..self.num_contacts as usize {
+            let elt = &self.elements[i].normal_part;
+            mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
+            mj_lambda2.angular += elt.gcross2 * elt.impulse;
+
+            for j in 0..DIM - 1 {
+                let elt = &self.elements[i].tangent_part[j];
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
+                mj_lambda2.angular += elt.gcross2 * elt.impulse;
+            }
+        }
+
+        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<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        // Solve friction.
+        let tangents1 = self.dir1.orthonormal_basis();
+
+        for i in 0..self.num_contacts as usize {
+            for j in 0..DIM - 1 {
+                let normal_elt = &self.elements[i].normal_part;
+                let elt = &mut self.elements[i].tangent_part[j];
+                let dimpulse = -tangents1[j].dot(&mj_lambda2.linear)
+                    + elt.gcross2.gdot(mj_lambda2.angular)
+                    + elt.rhs;
+                let limit = self.limit * normal_elt.impulse;
+                let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
+                let dlambda = new_impulse - elt.impulse;
+                elt.impulse = new_impulse;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
+                mj_lambda2.angular += elt.gcross2 * dlambda;
+            }
+        }
+
+        // Solve penetration.
+        for i in 0..self.num_contacts as usize {
+            let elt = &mut self.elements[i].normal_part;
+            let dimpulse =
+                -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs;
+            let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0);
+            let dlambda = new_impulse - elt.impulse;
+            elt.impulse = new_impulse;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
+            mj_lambda2.angular += elt.gcross2 * dlambda;
+        }
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        let manifold = &mut manifolds_all[self.manifold_id];
+        let k_base = self.manifold_contact_id;
+
+        for k in 0..self.num_contacts as usize {
+            let active_contacts = manifold.active_contacts_mut();
+            active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
+            #[cfg(feature = "dim2")]
+            {
+                active_contacts[k_base + k].tangent_impulse =
+                    self.elements[k].tangent_part[0].impulse;
+            }
+            #[cfg(feature = "dim3")]
+            {
+                active_contacts[k_base + k].tangent_impulse = [
+                    self.elements[k].tangent_part[0].impulse,
+                    self.elements[k].tangent_part[1].impulse,
+                ];
+            }
+        }
+    }
+}